
#include <cassert>

#ifndef I_PLATFORM_NO_SSE_SUPPORT
#include <pmmintrin.h>  // SSE3
#endif

#include "../core/i_blas.h"
#include "../core/i_alloc.h"
#include "../core/i_rand.h"
#include "../numeric/i_lu.h"
#include "../ip/i_integral.h"
#include "i_surf.h"
#include "i_match.h"

namespace idl
{
  InterestPointFastHessian::InterestPointFastHessian()
  {
    this->type = 0;
    this->x = this->y = this->scale = this->orientation = this->response = 0.f;
    i_zero16<float>(this->descriptor);
    i_zero16<float>(this->descriptor + 16);
    i_zero16<float>(this->descriptor + 32);
    i_zero16<float>(this->descriptor + 48);
  }

  InterestPointFastHessian::~InterestPointFastHessian(){}

  /*Normalize the feature descriptor vector*/
  inline void InterestPointFastHessian::normalize()
  {
    i_unitize(this->descriptor, 64);
  }

  FastHessianResponseLayer::FastHessianResponseLayer(int width, int height, int step, int filter) : Base(width, height, Constant<float>::MAX_VAL())
  {
    this->step = step;
    this->filter = filter;
    this->lobe = filter / 3;
    this->radius = (filter - 1) / 2;
    this->area = filter*filter;
    int rdivstep = (radius / step);
    this->bound[0] = rdivstep;
    this->bound[1] = this->width - rdivstep;
    this->bound[2] = rdivstep;
    this->bound[3] = this->height - rdivstep;
    laplacian = i_alloc2<unsigned char>(height, width);
    if (!laplacian)
    {
      this->initialized = false;
    } else 
    {
      i_fill(laplacian[0], this->size, (unsigned char)0);//default is zero
    }
  }

  inline unsigned char FastHessianResponseLayer::get_laplacian(int x, int y) const
  {
    return laplacian[y][x];
  }
  inline void FastHessianResponseLayer::set_laplacian(int x, int y, unsigned char val)
  {
    laplacian[y][x] = val;
  }

  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  FastHessianDetector::FastHessianDetector(const int width, const int height,
    const int octaves, const int init_sample) :_width(width), _height(height), _octaves(octaves)
  {
    //alloc memory for integral image
    register int step = init_sample;
    register int w = width / step;
    register int h = height / step;
    i_fill<FastHessianResponseLayer*>(_layers, MAX_NR_RESPONSE_LAYERS , (FastHessianResponseLayer *)NULL);

    _nr_layers = 0;
    if (octaves >= 1)
    {
      _layers[0] = new FastHessianResponseLayer(w, h, step, 9);
      _layers[1] = new FastHessianResponseLayer(w, h, step, 15);
      _layers[2] = new FastHessianResponseLayer(w, h, step, 21);
      _layers[3] = new FastHessianResponseLayer(w, h, step, 27);
      _nr_layers += 4;
    }
    if (octaves >= 2)
    {
      w /= 2; h /= 2;
      step *= 2;
      _layers[4] = new FastHessianResponseLayer(w, h, step, 39);
      _layers[5] = new FastHessianResponseLayer(w, h, step, 51);
      _nr_layers += 2;
    }
    if (octaves >= 3)
    {
      w /= 2; h /= 2;
      step *= 2;
      _layers[6] = new FastHessianResponseLayer(w, h, step, 75);
      _layers[7] = new FastHessianResponseLayer(w, h, step, 99);
      _nr_layers += 2;
    }
    if (octaves >= 4)
    {
      w /= 2; h /= 2;
      step *= 2;
      _layers[8] = new FastHessianResponseLayer(w, h, step, 147);
      _layers[9] = new FastHessianResponseLayer(w, h, step, 195);
      _nr_layers += 2;
    }
    if (octaves >= 5)
    {
      w /= 2; h /= 2;
      step *= 2;
      _layers[10] = new FastHessianResponseLayer(w, h, step, 291);
      _layers[11] = new FastHessianResponseLayer(w, h, step, 387);
      _nr_layers += 2;
    }
    _response_thres = 0.f;//initialize to zero.
  }

  FastHessianDetector::~FastHessianDetector()
  {
    for (int i = 0; i < MAX_NR_RESPONSE_LAYERS; ++i)
    {
      if (_layers[i] != (FastHessianResponseLayer *)NULL)
      {
        delete _layers[i];
        _layers[i] = (FastHessianResponseLayer *)NULL;
      }
    }
  }

  int FastHessianDetector::detect(const float *const *integral, std::vector<InterestPointFastHessian> &ipts, const float thres)
  {
    ipts.clear();
    ipts.reserve(I_DEFAULT_NR_INTEREST_POINTS_PER_IMAGE);
    _response_thres = thres; //update each time;
    const int index_map[MAX_NR_OCTAVES][4] = { { 0, 1, 2, 3 }, { 1, 3, 4, 5 }, { 3, 5, 6, 7 }, { 5, 7, 8, 9 }, { 7, 9, 10, 11 } };
    int nr_ipts = 0;
    //check error: not valid input image;
#ifndef NDEBUG
    if (!integral){ return -1; }
#endif
    //construct Hessian response layers:
    for (int l = 0; l < _nr_layers; ++l)
    {
      buildHessianResponseLayer(integral, _layers[l]);
    }
    const FastHessianResponseLayer *up = NULL, *ct = NULL, *dn = NULL;
    for (int o = 0; o < _octaves; o++)
    {
      up = _layers[index_map[o][0]];
      ct = _layers[index_map[o][1]];
      dn = _layers[index_map[o][2]];
      nr_ipts += extratExtrema(up, ct, dn, ipts);

      up = _layers[index_map[o][1]];
      ct = _layers[index_map[o][2]];
      dn = _layers[index_map[o][3]];
      nr_ipts += extratExtrema(up, ct, dn, ipts);
    }
    return (int)ipts.size();
  }
  int FastHessianDetector::detect(const float *const *integral, std::vector<InterestPointBrief128> &ipts, const float thres)
  {
    ipts.clear();
    ipts.reserve(I_DEFAULT_NR_INTEREST_POINTS_PER_IMAGE);
    _response_thres = thres; //update each time;
    const int index_map[MAX_NR_OCTAVES][4] = { { 0, 1, 2, 3 }, { 1, 3, 4, 5 }, { 3, 5, 6, 7 }, { 5, 7, 8, 9 }, { 7, 9, 10, 11 } };
    int nr_ipts = 0;
    //check error: not valid input image;
#ifndef NDEBUG
    if (!integral){ return -1; }
#endif
    //construct Hessian response layers:
    for (int l = 0; l < _nr_layers; ++l)
    {
      buildHessianResponseLayer(integral, _layers[l]);
    }
    const FastHessianResponseLayer *up = NULL, *ct = NULL, *dn = NULL;
    for (int o = 0; o < _octaves; o++)
    {
      up = _layers[index_map[o][0]];
      ct = _layers[index_map[o][1]];
      dn = _layers[index_map[o][2]];
      nr_ipts += extratExtrema(up, ct, dn, ipts);

      up = _layers[index_map[o][1]];
      ct = _layers[index_map[o][2]];
      dn = _layers[index_map[o][3]];
      nr_ipts += extratExtrema(up, ct, dn, ipts);
    }
    return (int)ipts.size();
  }
  int FastHessianDetector::detect(const float *const *integral, std::vector<InterestPointBrief256> &ipts, const float thres)
  {
    ipts.clear();
    ipts.reserve(I_DEFAULT_NR_INTEREST_POINTS_PER_IMAGE);
    _response_thres = thres; //update each time;
    const int index_map[MAX_NR_OCTAVES][4] = { { 0, 1, 2, 3 }, { 1, 3, 4, 5 }, { 3, 5, 6, 7 }, { 5, 7, 8, 9 }, { 7, 9, 10, 11 } };
    int nr_ipts = 0;
    //check error: not valid input image;
#ifndef NDEBUG
    if (!integral){ return -1; }
#endif
    //construct Hessian response layers:
    for (int l = 0; l < _nr_layers; ++l)
    {
      buildHessianResponseLayer(integral, _layers[l]);
    }
    const FastHessianResponseLayer *up = NULL, *ct = NULL, *dn = NULL;
    for (int o = 0; o < _octaves; o++)
    {
      up = _layers[index_map[o][0]];
      ct = _layers[index_map[o][1]];
      dn = _layers[index_map[o][2]];
      nr_ipts += extratExtrema(up, ct, dn, ipts);

      up = _layers[index_map[o][1]];
      ct = _layers[index_map[o][2]];
      dn = _layers[index_map[o][3]];
      nr_ipts += extratExtrema(up, ct, dn, ipts);
    }
    return (int)ipts.size();
  }
  int FastHessianDetector::detect(const float *const *integral, std::vector<InterestPointBrief512> &ipts, const float thres)
  {
    ipts.clear();
    ipts.reserve(I_DEFAULT_NR_INTEREST_POINTS_PER_IMAGE);
    _response_thres = thres; //update each time;
    const int index_map[MAX_NR_OCTAVES][4] = { { 0, 1, 2, 3 }, { 1, 3, 4, 5 }, { 3, 5, 6, 7 }, { 5, 7, 8, 9 }, { 7, 9, 10, 11 } };
    int nr_ipts = 0;
    //check error: not valid input image;
#ifndef NDEBUG
    if (!integral){ return -1; }
#endif
    //construct Hessian response layers:
    for (int l = 0; l < _nr_layers; ++l)
    {
      buildHessianResponseLayer(integral, _layers[l]);
    }
    const FastHessianResponseLayer *up = NULL, *ct = NULL, *dn = NULL;
    for (int o = 0; o < _octaves; o++)
    {
      up = _layers[index_map[o][0]];
      ct = _layers[index_map[o][1]];
      dn = _layers[index_map[o][2]];
      nr_ipts += extratExtrema(up, ct, dn, ipts);

      up = _layers[index_map[o][1]];
      ct = _layers[index_map[o][2]];
      dn = _layers[index_map[o][3]];
      nr_ipts += extratExtrema(up, ct, dn, ipts);
    }
    return (int)ipts.size();
  }

  void FastHessianDetector::buildHessianResponseLayer(const float *const *integral, FastHessianResponseLayer* layer)
  {
#ifndef NDEBUG
    if (!integral || !layer){ return; }
#endif
    const int begin_c = layer->bound[0];
    const int end_c = layer->bound[1];
    const int begin_r = layer->bound[2];
    const int end_r = layer->bound[3];
    const int begin_x = begin_c*layer->step;
    const int begin_y = begin_r*layer->step;

    register int step = layer->step;                      // step size for this filter
    register int b = layer->radius;             // filter radius
    register int l = layer->lobe;                   // lobe for this filter (filter size / 3)
    int l2m1 = l + l - 1;
    int l_half = l / 2;
    register int f = layer->filter;                       // filter size
    register float inverse_area = (float)i_rec(layer->area);        // normalization factor
    register float dxdx, dydy, dxdy;
    register int r, c, x = 0, y = 0;                      // (r,c) -> layer coordinates, (x,y) -> image coordinates
    register int ymb, yml, yp1, ymlp1, ymlhalf, xml, xp1, xmlp1;

    for (r = begin_r, y = begin_y; r < end_r; ++r, y += step)
    {
      if (y>=_height){ break; }
      ymb = y - b;
      yml = y - l;
      yp1 = y + 1;
      ymlp1 = yml + 1;
      ymlhalf = y - l_half;
      for (c = begin_c, x = begin_x; c < end_c; ++c, x += step)
      {
        if (x>=_width){ break; }
        xml = x - l;
        xp1 = x + 1;
        xmlp1 = xml + 1;
        // Compute response components
        dxdx = i_integral_rect(integral, x - b, ymlp1, f, l2m1, _width, _height) - 3 * i_integral_rect(integral, x - l_half, ymlp1, l, l2m1, _width, _height);
        dydy = i_integral_rect(integral, xmlp1, ymb, l2m1, f, _width, _height) - 3 * i_integral_rect(integral, xmlp1, ymlhalf, l2m1, l, _width, _height);
        dxdy = i_integral_rect(integral, xml, yml, l, l, _width, _height) +
               i_integral_rect(integral, xp1, yp1, l, l, _width, _height) -
               i_integral_rect(integral, xp1, yml, l, l, _width, _height) -
               i_integral_rect(integral, xml, yp1, l, l, _width, _height);
        // Normalise the filter responses with respect to their size
        dxdx *= inverse_area;
        dydy *= inverse_area;
        dxdy *= inverse_area;
        // Get the determinant of hessian response & laplacian sign
        (*layer)(r, c) = (dxdx * dydy - 0.81f * dxdy * dxdy);
        if (dxdx + dydy >= 0.f)
        {
          layer->set_laplacian(c, r, 1); //by default is zero
        }
      }
    }
  }

  // Non Maximal Suppression in 3x3x3 volume SURF
  int FastHessianDetector::extratExtrema(const FastHessianResponseLayer *up,
                                         const FastHessianResponseLayer *ct,
                                         const FastHessianResponseLayer *dn,
                                         std::vector<InterestPointFastHessian>& ipts)
  {
    if (up->size == ct->size && ct->size == dn->size) 
    {
      return extratExtrema_1(*up, *ct, *dn, ipts);
    }
    else if (up->size == ct->size && ct->size > dn->size)
    {
      return extratExtrema_2(*up, *ct, *dn, ipts);
    }
    else if (up->size > ct->size && ct->size == dn->size)
    {
      return extratExtrema_3(*up, *ct, *dn, ipts);
    }
    else if (up->size > ct->size && ct->size > dn->size)
    {
      return extratExtrema_4(*up, *ct, *dn, ipts);
    }
    else
    {
      return 0;/*impossible actually*/
    }
  }

  int FastHessianDetector::extratExtrema_1(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointFastHessian>& ipts)
  {
    assert(up.size == ct.size && ct.size == dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;

    register int x, y, ym1, yp1, xm1, xp1;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        response = ct(y, x);
        if (response < _response_thres)
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
            response <= ct(y, xm1)   || response <= ct(y, xp1) ||
            response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(ym1, xm1) || response <= up(ym1, x) || response <= up(ym1, xp1) ||
            response <= up(y, xm1) || response <= up(y, x) || response <= up(y, xp1) ||
            response <= up(yp1, xm1) || response <= up(yp1, x) || response <= up(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(ym1, xm1) || response <= dn(ym1, x) || response <= dn(ym1, xp1) ||
            response <= dn(y,   xm1) || response <= dn(y, x) || response <= dn(y, xp1) ||
            response <= dn(yp1, xm1) || response <= dn(yp1, x) || response <= dn(yp1, xp1))
        {
          continue;
        }
        response2 = (response + response);

        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(y, x) - up(y, x))     * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(y, x) + up(y, x) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(y, xp1) - dn(y, xm1) - up(y, xp1) + up(y, xm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yp1, x) - dn(ym1, x) - up(yp1, x) + up(ym1, x))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointFastHessian ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_2(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointFastHessian>& ipts)
  {
    assert(up.size == ct.size && ct.size > dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1, xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int dwm2 = dn.width - 2;
    register int dhm2 = dn.height - 2;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = i_round((float)y*0.5f);
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = i_round((float)x*0.5f);
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, dwm2, dhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
            response <= ct(y,   xm1) || response <= ct(y, xp1) ||
            response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(ym1, xm1) || response <= up(ym1, x) || response <= up(ym1, xp1) ||
            response <= up(y,   xm1) || response <= up(y,   x) || response <= up(y,   xp1) ||
            response <= up(yp1, xm1) || response <= up(yp1, x) || response <= up(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(yhm1, xhm1) || response <= dn(yhm1, xh) || response <= dn(yhm1, xhp1) ||
            response <= dn(yh,   xhm1) || response <= dn(yh,   xh) || response <= dn(yh,   xhp1) ||
            response <= dn(yhp1, xhm1) || response <= dn(yhp1, xh) || response <= dn(yhp1, xhp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(yh, xh) - up(y, x))   * 0.5f;   //ds
        
        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(yh, xh) + up(y, x) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(yh, xhp1) - dn(yh, xhm1) - up(y,   xp1) + up(y,   xm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yhp1, xh) - dn(yhm1, xh) - up(yp1,   x) + up(ym1,   x))*0.25f; //dyds
        
        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);
        
        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointFastHessian ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_3(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointFastHessian>& ipts)
  {
    assert(up.size > ct.size && ct.size == dn.size); 
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1, xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int uwm2 = up.width - 2;
    register int uhm2 = up.height - 2;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = y + y;
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = x + x;
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, uwm2, uhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
            response <= ct(y,   xm1) || response <= ct(y, xp1) ||
            response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(ym1, xm1) || response <= dn(ym1, x) || response <= dn(ym1, xp1) ||
            response <= dn(y,   xm1) || response <= dn(y,   x) || response <= dn(y,   xp1) ||
            response <= dn(yp1, xm1) || response <= dn(yp1, x) || response <= dn(yp1, xp1))
        {
          continue;
        }
        if (response <= up(yhm1, xhm1) || response <= up(yhm1, xh) || response <= up(yhm1, xhp1) ||
            response <= up(yh,   xhm1) || response <= up(yh,   xh) || response <= up(yh,   xhp1) ||
            response <= up(yhp1, xhm1) || response <= up(yhp1, xh) || response <= up(yhp1, xhp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(y,   x) - up(yh, xh)) * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(y, x) + up(yh, xh) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(y,   xp1) - dn(y,   xm1) - up(yh, xhp1) + up(yh, xhm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yp1,   x) - dn(ym1,   x) - up(yhp1, xh) + up(yhm1, xh))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointFastHessian ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_4(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointFastHessian>& ipts)
  {
    assert(up.size > ct.size && ct.size > dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x,  y,  ym1,  yp1,  xm1,  xp1; 
    register int xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int xp, yp, xpm1, xpp1, ypm1, ypp1;
    register int uwm2 = up.width - 2;
    register int uhm2 = up.height - 2;
    register int dwm2 = dn.width - 2;
    register int dhm2 = dn.height - 2;
    register float response = 0.f, response2 = 0.f;
    
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = y + y;
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      yp = i_round((float)y*0.5f);
      ypm1 = yp - 1;
      ypp1 = yp + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = x + x;
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        xp = i_round((float)x*0.5f);
        xpm1 = xp - 1;
        xpp1 = xp + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, uwm2, uhm2) || !i_within_2d(xp, yp, 1, 1, dwm2, dhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
            response <= ct(y, xm1)   || response <= ct(y, xp1) ||
            response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(yhm1, xhm1) || response <= up(yhm1, xh) || response <= up(yhm1, xhp1) ||
            response <= up(yh,   xhm1) || response <= up(yh,   xh) || response <= up(yh,   xhp1) ||
            response <= up(yhp1, xhm1) || response <= up(yhp1, xh) || response <= up(yhp1, xhp1))
        {
          continue;
        }
        if (response <= dn(ypm1, xpm1) || response <= dn(ypm1, xp) || response <= dn(ypm1, xpp1) ||
            response <= dn(yp,   xpm1) || response <= dn(yp,   xp) || response <= dn(yp,   xpp1) ||
            response <= dn(ypp1, xpm1) || response <= dn(ypp1, xp) || response <= dn(ypp1, xpp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(yp, xp) - up(yh, xh)) * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(yp, xp) + up(yh, xh) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(yp, xpp1) - dn(yp, xpm1) - up(yh, xhp1) + up(yh, xhm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(ypp1, xp) - dn(ypm1, xp) - up(yhp1, xh) + up(yhm1, xh))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointFastHessian ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }


  // Non Maximal Suppression in 3x3x3 volume BRIEF-128
  int FastHessianDetector::extratExtrema(const FastHessianResponseLayer *up,
                                         const FastHessianResponseLayer *ct,
                                         const FastHessianResponseLayer *dn,
                                         std::vector<InterestPointBrief128>& ipts)
  {
    if (up->size == ct->size && ct->size == dn->size)
    {
      return extratExtrema_1(*up, *ct, *dn, ipts);
    }
    else if (up->size == ct->size && ct->size > dn->size)
    {
      return extratExtrema_2(*up, *ct, *dn, ipts);
    }
    else if (up->size > ct->size && ct->size == dn->size)
    {
      return extratExtrema_3(*up, *ct, *dn, ipts);
    }
    else if (up->size > ct->size && ct->size > dn->size)
    {
      return extratExtrema_4(*up, *ct, *dn, ipts);
    }
    else
    {
      return 0;/*impossible actually*/
    }
  }

  int FastHessianDetector::extratExtrema_1(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief128>& ipts)
  {
    assert(up.size == ct.size && ct.size == dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;

    register int x, y, ym1, yp1, xm1, xp1;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        response = ct(y, x);
        if (response < _response_thres)
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(ym1, xm1) || response <= up(ym1, x) || response <= up(ym1, xp1) ||
          response <= up(y, xm1) || response <= up(y, x) || response <= up(y, xp1) ||
          response <= up(yp1, xm1) || response <= up(yp1, x) || response <= up(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(ym1, xm1) || response <= dn(ym1, x) || response <= dn(ym1, xp1) ||
          response <= dn(y, xm1) || response <= dn(y, x) || response <= dn(y, xp1) ||
          response <= dn(yp1, xm1) || response <= dn(yp1, x) || response <= dn(yp1, xp1))
        {
          continue;
        }
        response2 = (response + response);

        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(y, x) - up(y, x))     * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(y, x) + up(y, x) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(y, xp1) - dn(y, xm1) - up(y, xp1) + up(y, xm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yp1, x) - dn(ym1, x) - up(yp1, x) + up(ym1, x))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief128 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_2(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief128>& ipts)
  {
    assert(up.size == ct.size && ct.size > dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1, xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int dwm2 = dn.width - 2;
    register int dhm2 = dn.height - 2;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = i_round((float)y*0.5f);
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = i_round((float)x*0.5f);
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, dwm2, dhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(ym1, xm1) || response <= up(ym1, x) || response <= up(ym1, xp1) ||
          response <= up(y, xm1) || response <= up(y, x) || response <= up(y, xp1) ||
          response <= up(yp1, xm1) || response <= up(yp1, x) || response <= up(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(yhm1, xhm1) || response <= dn(yhm1, xh) || response <= dn(yhm1, xhp1) ||
          response <= dn(yh, xhm1) || response <= dn(yh, xh) || response <= dn(yh, xhp1) ||
          response <= dn(yhp1, xhm1) || response <= dn(yhp1, xh) || response <= dn(yhp1, xhp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(yh, xh) - up(y, x))   * 0.5f;   //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(yh, xh) + up(y, x) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(yh, xhp1) - dn(yh, xhm1) - up(y, xp1) + up(y, xm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yhp1, xh) - dn(yhm1, xh) - up(yp1, x) + up(ym1, x))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief128 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_3(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief128>& ipts)
  {
    assert(up.size > ct.size && ct.size == dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1, xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int uwm2 = up.width - 2;
    register int uhm2 = up.height - 2;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = y + y;
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = x + x;
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, uwm2, uhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(ym1, xm1) || response <= dn(ym1, x) || response <= dn(ym1, xp1) ||
          response <= dn(y, xm1) || response <= dn(y, x) || response <= dn(y, xp1) ||
          response <= dn(yp1, xm1) || response <= dn(yp1, x) || response <= dn(yp1, xp1))
        {
          continue;
        }
        if (response <= up(yhm1, xhm1) || response <= up(yhm1, xh) || response <= up(yhm1, xhp1) ||
          response <= up(yh, xhm1) || response <= up(yh, xh) || response <= up(yh, xhp1) ||
          response <= up(yhp1, xhm1) || response <= up(yhp1, xh) || response <= up(yhp1, xhp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(y, x) - up(yh, xh)) * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(y, x) + up(yh, xh) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(y, xp1) - dn(y, xm1) - up(yh, xhp1) + up(yh, xhm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yp1, x) - dn(ym1, x) - up(yhp1, xh) + up(yhm1, xh))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief128 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_4(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief128>& ipts)
  {
    assert(up.size > ct.size && ct.size > dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1;
    register int xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int xp, yp, xpm1, xpp1, ypm1, ypp1;
    register int uwm2 = up.width - 2;
    register int uhm2 = up.height - 2;
    register int dwm2 = dn.width - 2;
    register int dhm2 = dn.height - 2;
    register float response = 0.f, response2 = 0.f;

    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = y + y;
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      yp = i_round((float)y*0.5f);
      ypm1 = yp - 1;
      ypp1 = yp + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = x + x;
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        xp = i_round((float)x*0.5f);
        xpm1 = xp - 1;
        xpp1 = xp + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, uwm2, uhm2) || !i_within_2d(xp, yp, 1, 1, dwm2, dhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(yhm1, xhm1) || response <= up(yhm1, xh) || response <= up(yhm1, xhp1) ||
          response <= up(yh, xhm1) || response <= up(yh, xh) || response <= up(yh, xhp1) ||
          response <= up(yhp1, xhm1) || response <= up(yhp1, xh) || response <= up(yhp1, xhp1))
        {
          continue;
        }
        if (response <= dn(ypm1, xpm1) || response <= dn(ypm1, xp) || response <= dn(ypm1, xpp1) ||
          response <= dn(yp, xpm1) || response <= dn(yp, xp) || response <= dn(yp, xpp1) ||
          response <= dn(ypp1, xpm1) || response <= dn(ypp1, xp) || response <= dn(ypp1, xpp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(yp, xp) - up(yh, xh)) * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(yp, xp) + up(yh, xh) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(yp, xpp1) - dn(yp, xpm1) - up(yh, xhp1) + up(yh, xhm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(ypp1, xp) - dn(ypm1, xp) - up(yhp1, xh) + up(yhm1, xh))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief128 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  // Non Maximal Suppression in 3x3x3 volume BRIEF-256
  int FastHessianDetector::extratExtrema(const FastHessianResponseLayer *up,
                                         const FastHessianResponseLayer *ct,
                                         const FastHessianResponseLayer *dn,
                                         std::vector<InterestPointBrief256>& ipts)
  {
    if (up->size == ct->size && ct->size == dn->size)
    {
      return extratExtrema_1(*up, *ct, *dn, ipts);
    }
    else if (up->size == ct->size && ct->size > dn->size)
    {
      return extratExtrema_2(*up, *ct, *dn, ipts);
    }
    else if (up->size > ct->size && ct->size == dn->size)
    {
      return extratExtrema_3(*up, *ct, *dn, ipts);
    }
    else if (up->size > ct->size && ct->size > dn->size)
    {
      return extratExtrema_4(*up, *ct, *dn, ipts);
    }
    else
    {
      return 0;/*impossible actually*/
    }
  }

  int FastHessianDetector::extratExtrema_1(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief256>& ipts)
  {
    assert(up.size == ct.size && ct.size == dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;

    register int x, y, ym1, yp1, xm1, xp1;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        response = ct(y, x);
        if (response < _response_thres)
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(ym1, xm1) || response <= up(ym1, x) || response <= up(ym1, xp1) ||
          response <= up(y, xm1) || response <= up(y, x) || response <= up(y, xp1) ||
          response <= up(yp1, xm1) || response <= up(yp1, x) || response <= up(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(ym1, xm1) || response <= dn(ym1, x) || response <= dn(ym1, xp1) ||
          response <= dn(y, xm1) || response <= dn(y, x) || response <= dn(y, xp1) ||
          response <= dn(yp1, xm1) || response <= dn(yp1, x) || response <= dn(yp1, xp1))
        {
          continue;
        }
        response2 = (response + response);

        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(y, x) - up(y, x))     * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(y, x) + up(y, x) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(y, xp1) - dn(y, xm1) - up(y, xp1) + up(y, xm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yp1, x) - dn(ym1, x) - up(yp1, x) + up(ym1, x))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief256 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_2(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief256>& ipts)
  {
    assert(up.size == ct.size && ct.size > dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1, xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int dwm2 = dn.width - 2;
    register int dhm2 = dn.height - 2;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = i_round((float)y*0.5f);
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = i_round((float)x*0.5f);
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, dwm2, dhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(ym1, xm1) || response <= up(ym1, x) || response <= up(ym1, xp1) ||
          response <= up(y, xm1) || response <= up(y, x) || response <= up(y, xp1) ||
          response <= up(yp1, xm1) || response <= up(yp1, x) || response <= up(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(yhm1, xhm1) || response <= dn(yhm1, xh) || response <= dn(yhm1, xhp1) ||
          response <= dn(yh, xhm1) || response <= dn(yh, xh) || response <= dn(yh, xhp1) ||
          response <= dn(yhp1, xhm1) || response <= dn(yhp1, xh) || response <= dn(yhp1, xhp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(yh, xh) - up(y, x))   * 0.5f;   //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(yh, xh) + up(y, x) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(yh, xhp1) - dn(yh, xhm1) - up(y, xp1) + up(y, xm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yhp1, xh) - dn(yhm1, xh) - up(yp1, x) + up(ym1, x))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief256 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_3(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief256>& ipts)
  {
    assert(up.size > ct.size && ct.size == dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1, xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int uwm2 = up.width - 2;
    register int uhm2 = up.height - 2;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = y + y;
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = x + x;
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, uwm2, uhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(ym1, xm1) || response <= dn(ym1, x) || response <= dn(ym1, xp1) ||
          response <= dn(y, xm1) || response <= dn(y, x) || response <= dn(y, xp1) ||
          response <= dn(yp1, xm1) || response <= dn(yp1, x) || response <= dn(yp1, xp1))
        {
          continue;
        }
        if (response <= up(yhm1, xhm1) || response <= up(yhm1, xh) || response <= up(yhm1, xhp1) ||
          response <= up(yh, xhm1) || response <= up(yh, xh) || response <= up(yh, xhp1) ||
          response <= up(yhp1, xhm1) || response <= up(yhp1, xh) || response <= up(yhp1, xhp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(y, x) - up(yh, xh)) * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(y, x) + up(yh, xh) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(y, xp1) - dn(y, xm1) - up(yh, xhp1) + up(yh, xhm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yp1, x) - dn(ym1, x) - up(yhp1, xh) + up(yhm1, xh))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief256 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_4(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief256>& ipts)
  {
    assert(up.size > ct.size && ct.size > dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1;
    register int xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int xp, yp, xpm1, xpp1, ypm1, ypp1;
    register int uwm2 = up.width - 2;
    register int uhm2 = up.height - 2;
    register int dwm2 = dn.width - 2;
    register int dhm2 = dn.height - 2;
    register float response = 0.f, response2 = 0.f;

    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = y + y;
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      yp = i_round((float)y*0.5f);
      ypm1 = yp - 1;
      ypp1 = yp + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = x + x;
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        xp = i_round((float)x*0.5f);
        xpm1 = xp - 1;
        xpp1 = xp + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, uwm2, uhm2) || !i_within_2d(xp, yp, 1, 1, dwm2, dhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(yhm1, xhm1) || response <= up(yhm1, xh) || response <= up(yhm1, xhp1) ||
          response <= up(yh, xhm1) || response <= up(yh, xh) || response <= up(yh, xhp1) ||
          response <= up(yhp1, xhm1) || response <= up(yhp1, xh) || response <= up(yhp1, xhp1))
        {
          continue;
        }
        if (response <= dn(ypm1, xpm1) || response <= dn(ypm1, xp) || response <= dn(ypm1, xpp1) ||
          response <= dn(yp, xpm1) || response <= dn(yp, xp) || response <= dn(yp, xpp1) ||
          response <= dn(ypp1, xpm1) || response <= dn(ypp1, xp) || response <= dn(ypp1, xpp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(yp, xp) - up(yh, xh)) * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(yp, xp) + up(yh, xh) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(yp, xpp1) - dn(yp, xpm1) - up(yh, xhp1) + up(yh, xhm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(ypp1, xp) - dn(ypm1, xp) - up(yhp1, xh) + up(yhm1, xh))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief256 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  // Non Maximal Suppression in 3x3x3 volume BRIEF-512
  int FastHessianDetector::extratExtrema(const FastHessianResponseLayer *up,
                                         const FastHessianResponseLayer *ct,
                                         const FastHessianResponseLayer *dn,
                                         std::vector<InterestPointBrief512>& ipts)
  {
    if (up->size == ct->size && ct->size == dn->size)
    {
      return extratExtrema_1(*up, *ct, *dn, ipts);
    }
    else if (up->size == ct->size && ct->size > dn->size)
    {
      return extratExtrema_2(*up, *ct, *dn, ipts);
    }
    else if (up->size > ct->size && ct->size == dn->size)
    {
      return extratExtrema_3(*up, *ct, *dn, ipts);
    }
    else if (up->size > ct->size && ct->size > dn->size)
    {
      return extratExtrema_4(*up, *ct, *dn, ipts);
    }
    else
    {
      return 0;/*impossible actually*/
    }
  }

  int FastHessianDetector::extratExtrema_1(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief512>& ipts)
  {
    assert(up.size == ct.size && ct.size == dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;

    register int x, y, ym1, yp1, xm1, xp1;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        response = ct(y, x);
        if (response < _response_thres)
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(ym1, xm1) || response <= up(ym1, x) || response <= up(ym1, xp1) ||
          response <= up(y, xm1) || response <= up(y, x) || response <= up(y, xp1) ||
          response <= up(yp1, xm1) || response <= up(yp1, x) || response <= up(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(ym1, xm1) || response <= dn(ym1, x) || response <= dn(ym1, xp1) ||
          response <= dn(y, xm1) || response <= dn(y, x) || response <= dn(y, xp1) ||
          response <= dn(yp1, xm1) || response <= dn(yp1, x) || response <= dn(yp1, xp1))
        {
          continue;
        }
        response2 = (response + response);

        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(y, x) - up(y, x))     * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(y, x) + up(y, x) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(y, xp1) - dn(y, xm1) - up(y, xp1) + up(y, xm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yp1, x) - dn(ym1, x) - up(yp1, x) + up(ym1, x))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief512 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_2(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief512>& ipts)
  {
    assert(up.size == ct.size && ct.size > dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1, xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int dwm2 = dn.width - 2;
    register int dhm2 = dn.height - 2;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = i_round((float)y*0.5f);
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = i_round((float)x*0.5f);
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, dwm2, dhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(ym1, xm1) || response <= up(ym1, x) || response <= up(ym1, xp1) ||
          response <= up(y, xm1) || response <= up(y, x) || response <= up(y, xp1) ||
          response <= up(yp1, xm1) || response <= up(yp1, x) || response <= up(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(yhm1, xhm1) || response <= dn(yhm1, xh) || response <= dn(yhm1, xhp1) ||
          response <= dn(yh, xhm1) || response <= dn(yh, xh) || response <= dn(yh, xhp1) ||
          response <= dn(yhp1, xhm1) || response <= dn(yhp1, xh) || response <= dn(yhp1, xhp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(yh, xh) - up(y, x))   * 0.5f;   //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(yh, xh) + up(y, x) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(yh, xhp1) - dn(yh, xhm1) - up(y, xp1) + up(y, xm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yhp1, xh) - dn(yhm1, xh) - up(yp1, x) + up(ym1, x))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief512 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_3(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief512>& ipts)
  {
    assert(up.size > ct.size && ct.size == dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1, xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int uwm2 = up.width - 2;
    register int uhm2 = up.height - 2;
    register float response = 0.f, response2 = 0.f;
    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = y + y;
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = x + x;
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, uwm2, uhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= dn(ym1, xm1) || response <= dn(ym1, x) || response <= dn(ym1, xp1) ||
          response <= dn(y, xm1) || response <= dn(y, x) || response <= dn(y, xp1) ||
          response <= dn(yp1, xm1) || response <= dn(yp1, x) || response <= dn(yp1, xp1))
        {
          continue;
        }
        if (response <= up(yhm1, xhm1) || response <= up(yhm1, xh) || response <= up(yhm1, xhp1) ||
          response <= up(yh, xhm1) || response <= up(yh, xh) || response <= up(yh, xhp1) ||
          response <= up(yhp1, xhm1) || response <= up(yhp1, xh) || response <= up(yhp1, xhp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(y, x) - up(yh, xh)) * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(y, x) + up(yh, xh) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(y, xp1) - dn(y, xm1) - up(yh, xhp1) + up(yh, xhm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(yp1, x) - dn(ym1, x) - up(yhp1, xh) + up(yhm1, xh))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief512 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  int FastHessianDetector::extratExtrema_4(const FastHessianResponseLayer& up,
                                           const FastHessianResponseLayer& ct,
                                           const FastHessianResponseLayer& dn,
                                           std::vector<InterestPointBrief512>& ipts)
  {
    assert(up.size > ct.size && ct.size > dn.size);
    const int w = ct.width;
    const int h = ct.height;
    const int begin_x = i_max(1, ct.bound[0]);
    const int end_x = i_min(w - 1, ct.bound[1]);
    const int begin_y = i_max(1, ct.bound[2]);
    const int end_y = i_min(h - 1, ct.bound[3]);

    const float filterstep = (float)(ct.filter - up.filter);
    const float filterm3 = (float)(ct.filter - 3);
    assert(filterstep > 0);
    int nr_ipt = 0;
    register int x, y, ym1, yp1, xm1, xp1;
    register int xh, yh, xhm1, xhp1, yhm1, yhp1;
    register int xp, yp, xpm1, xpp1, ypm1, ypp1;
    register int uwm2 = up.width - 2;
    register int uhm2 = up.height - 2;
    register int dwm2 = dn.width - 2;
    register int dhm2 = dn.height - 2;
    register float response = 0.f, response2 = 0.f;

    float dxdyds[3], deris[3], hessi[9], hessi_i[9];

    for (y = begin_y; y < end_y; ++y)
    {
      ym1 = y - 1;
      yp1 = y + 1;
      yh = y + y;
      yhm1 = yh - 1;
      yhp1 = yh + 1;
      yp = i_round((float)y*0.5f);
      ypm1 = yp - 1;
      ypp1 = yp + 1;
      for (x = begin_x; x < end_x; ++x)
      {
        xm1 = x - 1;
        xp1 = x + 1;
        xh = x + x;
        xhm1 = xh - 1;
        xhp1 = xh + 1;
        xp = i_round((float)x*0.5f);
        xpm1 = xp - 1;
        xpp1 = xp + 1;
        response = ct(y, x);
        if (response < _response_thres || !i_within_2d(xh, yh, 1, 1, uwm2, uhm2) || !i_within_2d(xp, yp, 1, 1, dwm2, dhm2))
        {
          continue;
        }
        if (response <= ct(ym1, xm1) || response <= ct(ym1, x) || response <= ct(ym1, xp1) ||
          response <= ct(y, xm1) || response <= ct(y, xp1) ||
          response <= ct(yp1, xm1) || response <= ct(yp1, x) || response <= ct(yp1, xp1))
        {
          continue;
        }
        if (response <= up(yhm1, xhm1) || response <= up(yhm1, xh) || response <= up(yhm1, xhp1) ||
          response <= up(yh, xhm1) || response <= up(yh, xh) || response <= up(yh, xhp1) ||
          response <= up(yhp1, xhm1) || response <= up(yhp1, xh) || response <= up(yhp1, xhp1))
        {
          continue;
        }
        if (response <= dn(ypm1, xpm1) || response <= dn(ypm1, xp) || response <= dn(ypm1, xpp1) ||
          response <= dn(yp, xpm1) || response <= dn(yp, xp) || response <= dn(yp, xpp1) ||
          response <= dn(ypp1, xpm1) || response <= dn(ypp1, xp) || response <= dn(ypp1, xpp1))
        {
          continue;
        }
        response2 = response + response;
        //localize interest point:
        deris[0] = (ct(y, xp1) - ct(y, xm1)) * 0.5f; //dx
        deris[1] = (ct(yp1, x) - ct(ym1, x)) * 0.5f; //dy
        deris[2] = (dn(yp, xp) - up(yh, xh)) * 0.5f; //ds

        hessi[0] = ct(y, xp1) + ct(y, xm1) - response2; //dxdx
        hessi[4] = ct(yp1, x) + ct(ym1, x) - response2; //dydy
        hessi[8] = dn(yp, xp) + up(yh, xh) - response2; //dsds

        hessi[1] = hessi[3] = (ct(yp1, xp1) - ct(yp1, xm1) - ct(ym1, xp1) + ct(ym1, xm1))*0.25f; //dxdy
        hessi[2] = hessi[6] = (dn(yp, xpp1) - dn(yp, xpm1) - up(yh, xhp1) + up(yh, xhm1))*0.25f; //dxds
        hessi[5] = hessi[7] = (dn(ypp1, xp) - dn(ypm1, xp) - up(yhp1, xh) + up(yhm1, xh))*0.25f; //dyds

        //compute the inverse of hessian:
        i_lu_invert_3x3_destroy(hessi, hessi_i);//compute the inverse of Hessian using LU
        i_mult_Ax_3x3(hessi_i, deris, dxdyds);
        i_neg3(dxdyds);

        //check if the point is sufficiently close to the actual extremum, if yes, add it as a new interest point
        if (i_abs(dxdyds[0]) < 0.5f && i_abs(dxdyds[1]) < 0.5f && i_abs(dxdyds[2]) < 0.5f)
        {
          InterestPointBrief512 ipt;
          ipt.type = (int)ct.get_laplacian(x, y);
          ipt.x = ((float)x + dxdyds[0]) * ct.step;
          ipt.y = ((float)y + dxdyds[1]) * ct.step;
          ipt.scale = 0.133333f * (filterm3 + dxdyds[2] * filterstep);
          ipt.orientation = 0.f; //empty for now
          ipt.response = response;
          ipts.push_back(ipt);
          nr_ipt++;
        }
      }
    }
    return nr_ipt;
  }

  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*SURF Feature descriptor*/
  InterestPointDescriptorSurf::InterestPointDescriptorSurf(const int width, const int height) :_width(width), _height(height)
  {
    const float sigma1 = 2.5f; //2.5: default value given in the paper "CenSurE: Center Surround Extremas for Realtime Feature Detection and Matching, ECCV 2008" 
    const float sigma2 = 1.5f; //1.5: default value given in the paper "CenSurE: Center Surround Extremas for Realtime Feature Detection and Matching, ECCV 2008"
    _dx24 = i_alloc2<float>(24, 24);
    _dy24 = i_alloc2<float>(24, 24);
    _dx24abs = i_alloc2<float>(24, 24);
    _dy24abs = i_alloc2<float>(24, 24);
    i_gaussian_2d(_gaussian1, 9, sigma1);
    i_gaussian_2d(_gaussian2, 4, sigma2);
  }

  InterestPointDescriptorSurf::~InterestPointDescriptorSurf()
  {
    i_free2<float>(_dx24);
    i_free2<float>(_dy24);
    i_free2<float>(_dx24abs);
    i_free2<float>(_dy24abs);
  }
  
  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  inline void InterestPointDescriptorSurf::genSummedImagesUpright(const float *const *integral, InterestPointFastHessian &ipt)
  {
    float cx, cy, scale;
    int rcx, rcy, iscale, haarradius, regionradius, regionbegin_x, regionbegin_y, regionend_x, regionend_y;
    cx = ipt.x;
    cy = ipt.y;
    scale = ipt.scale;

    rcx = i_round(cx);
    rcy = i_round(cy);
    iscale = i_ceil(scale); //!!!seems perform better than i_round(scale);
    haarradius = i_round(scale); //i_ceil(scale); //round up scale -> the Haar wavelet filter radius - s
    regionradius = iscale * 12;  //region radius - 12s

    regionbegin_x = rcx - regionradius; // i_max(0, rcx - regionradius);
    regionbegin_y = rcy - regionradius; // i_max(0, rcy - regionradius);
    regionend_x   = rcx + regionradius;   // i_min(rcx + regionradius, _width);
    regionend_y   = rcy + regionradius;   // i_min(rcy + regionradius, _height);

    switch (iscale)
    {
    case 1: genSummedImagesUpright1x1(integral, regionbegin_x, regionbegin_y, regionend_x, regionend_y, haarradius); break;
    case 2: genSummedImagesUpright2x2(integral, regionbegin_x, regionbegin_y, regionend_x, regionend_y, haarradius); break;
    case 3: genSummedImagesUpright3x3(integral, regionbegin_x, regionbegin_y, regionend_x, regionend_y, haarradius); break;
    case 4: genSummedImagesUpright4x4(integral, regionbegin_x, regionbegin_y, regionend_x, regionend_y, haarradius); break;
    case 5: genSummedImagesUpright5x5(integral, regionbegin_x, regionbegin_y, regionend_x, regionend_y, haarradius); break;
    case 6: genSummedImagesUpright6x6(integral, regionbegin_x, regionbegin_y, regionend_x, regionend_y, haarradius); break;
    default: genSummedImagesUprightnxn(integral, regionbegin_x, regionbegin_y, regionend_x, regionend_y, haarradius, iscale); break; //this routine is slow
    }
  }

  inline void InterestPointDescriptorSurf::encodeDescriptor(InterestPointFastHessian &ipt)
  {
    encodeSubDescriptor00(ipt); encodeSubDescriptor01(ipt); encodeSubDescriptor02(ipt); encodeSubDescriptor03(ipt);
    encodeSubDescriptor10(ipt); encodeSubDescriptor11(ipt); encodeSubDescriptor12(ipt); encodeSubDescriptor13(ipt);
    encodeSubDescriptor20(ipt); encodeSubDescriptor21(ipt); encodeSubDescriptor22(ipt); encodeSubDescriptor23(ipt);
    encodeSubDescriptor30(ipt); encodeSubDescriptor31(ipt); encodeSubDescriptor32(ipt); encodeSubDescriptor33(ipt);
  }
  inline void  InterestPointDescriptorSurf::describeRotinva(const float *const *integral, InterestPointFastHessian &ipt)
  {
#ifndef NDEBUG
    if (!integral){ return; }
    ipt.x = ipt.y = 0.f;
#endif
  }
  
  inline void InterestPointDescriptorSurf::describeUpright(const float *const *integral, InterestPointFastHessian &ipt)
  {
    genSummedImagesUpright(integral, ipt);
    encodeDescriptor(ipt);
    ipt.normalize();
  }

  void InterestPointDescriptorSurf::describe(const float *const *integral, std::vector<InterestPointFastHessian> &ipts, bool upright)
  {
#ifndef NDEBUG
    if (!integral){ return; }
#endif
    int i, j;
    int nr_ipts = (int)ipts.size();
    int nr_loops = nr_ipts/8;
    int nr_remainder = nr_ipts % 8;
    if (upright)
    {
      for (j = 0; j < nr_remainder; ++j)
      {
        describeUpright(integral, ipts.at(j));
      }
      for (i = 0, j = nr_remainder; i < nr_loops; ++i)
      {
        describeUpright(integral, ipts.at(j++));//1
        describeUpright(integral, ipts.at(j++));//2
        describeUpright(integral, ipts.at(j++));//3
        describeUpright(integral, ipts.at(j++));//4
        describeUpright(integral, ipts.at(j++));//5
        describeUpright(integral, ipts.at(j++));//6
        describeUpright(integral, ipts.at(j++));//7
        describeUpright(integral, ipts.at(j++));//8
      }
    }
    else
    {
      for (j = 0; j < nr_remainder; ++j)
      {
        describeRotinva(integral, ipts.at(j));
      }
      for (i = 0, j = nr_remainder; i < nr_loops; ++i)
      {
        describeRotinva(integral, ipts.at(j++));//1
        describeRotinva(integral, ipts.at(j++));//2
        describeRotinva(integral, ipts.at(j++));//3
        describeRotinva(integral, ipts.at(j++));//4
        describeRotinva(integral, ipts.at(j++));//5
        describeRotinva(integral, ipts.at(j++));//6
        describeRotinva(integral, ipts.at(j++));//7
        describeRotinva(integral, ipts.at(j++));//8
      }
    }
  }

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
  inline void InterestPointDescriptorSurf::genSubSummedImagesUprightnxn(const float *const *integral, int begin_x, int begin_y, int n, int nsqr/*n*n*/, int haarradius, int haarwidth, int row, int col)
  {
    int x, y, ymrad, xmrad;
    const int end_x = begin_x + n;
    const int end_y = begin_y + n;
    float gx = 0.f, gy = 0.f, gx24 = 0.f, gy24 = 0.f, gx24abs = 0.f, gy24abs = 0.f;
    for (y = begin_y; y < end_y; ++y)
    {
      ymrad = y - haarradius;
      for (x = begin_x; x < end_x; ++x)
      {
        xmrad = x - haarradius;
        gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) -
          i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
        gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) -
          i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
        gx24 += gx;
        gy24 += gy;
        gx24abs += i_abs(gx);
        gy24abs += i_abs(gy);
      }
    }
    const float sf = 1.f / nsqr;
    _dx24[row][col] = gx24 * sf;
    _dy24[row][col] = gy24 * sf;
    _dx24abs[row][col] = gx24abs * sf;
    _dy24abs[row][col] = gy24abs * sf;
  }

  inline void InterestPointDescriptorSurf::genSummedImagesUprightnxn(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius, int scale)
  {
    int r, c, i = 0, j;
    const int haarwidth = (haarradius << 1);
    const int scalesqr = scale*scale; //used for normalization
    for (r = begin_y; r < end_y; r += scale, i++)
    {
      for (c = begin_x, j = 0; c < end_x; c += scale, j++)
      {
        genSubSummedImagesUprightnxn(integral, c, r, scale, scalesqr, haarradius, haarwidth, i, j);
      }
    }
  }

  inline void InterestPointDescriptorSurf::genSubSummedImagesUpright1x1(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col)
  {
    int ymrad, xmrad;
    float gx = 0.f, gy = 0.f;
    xmrad = begin_x - haarradius;
    ymrad = begin_y - haarradius;
    gx = i_integral_rect(integral, begin_x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, begin_y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    _dx24[row][col] = gx;
    _dy24[row][col] = gy;
    _dx24abs[row][col] = i_abs(gx);
    _dy24abs[row][col] = i_abs(gy);
  }

  inline void InterestPointDescriptorSurf::genSummedImagesUpright1x1(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius)
  {
    int r, c, i = 0, j;
    const int haarwidth = (haarradius << 1);
    for (r = begin_y; r < end_y; r++, i++)
    {
      for (c = begin_x, j = 0; c < end_x; c++, j++)
      {
        genSubSummedImagesUpright1x1(integral, c, r, haarradius, haarwidth, i, j);
      }
    }
  }

  inline void InterestPointDescriptorSurf::genSubSummedImagesUpright2x2(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col)
  {
    int x, y, ymrad, xmrad;
    float gx = 0.f, gy = 0.f, gx24 = 0.f, gy24 = 0.f, gx24abs = 0.f, gy24abs = 0.f;
    //1: (x, y)
    x = begin_x;
    y = begin_y;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    
    //2: (x+1, y)
    x = begin_x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    
    //3: (x+1, y+1)
    y = begin_y + 1;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    
    //4: (x, y+1)
    x = begin_x;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);


    _dx24[row][col] = gx24 * 0.25f;
    _dy24[row][col] = gy24 * 0.25f;
    _dx24abs[row][col] = gx24abs * 0.25f;
    _dy24abs[row][col] = gy24abs * 0.25f;
  }

  inline void InterestPointDescriptorSurf::genSummedImagesUpright2x2(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius)
  {
    int r, c, i = 0, j;
    const int haarwidth = (haarradius << 1);
    for (r = begin_y; r < end_y; r+=2, i++)
    {
      for (c = begin_x, j = 0; c < end_x; c += 2, j++)
      {
        genSubSummedImagesUpright2x2(integral, c, r, haarradius, haarwidth, i, j);
      }
    }
  }

  inline void InterestPointDescriptorSurf::genSubSummedImagesUpright3x3(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col)
  {
    int x, y, ymrad, xmrad;
    float gx = 0.f, gy = 0.f, gx24 = 0.f, gy24 = 0.f, gx24abs = 0.f, gy24abs = 0.f;
    //1: (x, y)
    x = begin_x;
    y = begin_y;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //2: (x+1, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //3: (x+2, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    
    //4: (x, y+1)
    x = begin_x;
    y = begin_y + 1;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //5: (x+1, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //6: (x+2, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //7: (x, y+2)
    x = begin_x;
    y = begin_y + 2;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //8: (x+1, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //9: (x+2, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    static const float sf = 1.f / 9;
    _dx24[row][col] = gx24 * sf;
    _dy24[row][col] = gy24 * sf;
    _dx24abs[row][col] = gx24abs * sf;
    _dy24abs[row][col] = gy24abs * sf;
  }

  inline void InterestPointDescriptorSurf::genSummedImagesUpright3x3(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius)
  {
    int r, c, i = 0, j;
    const int haarwidth = (haarradius << 1);
    for (r = begin_y; r < end_y; r += 3, i++)
    {
      for (c = begin_x, j = 0; c < end_x; c += 3, j++)
      {
        genSubSummedImagesUpright3x3(integral, c, r, haarradius, haarwidth, i, j);
      }
    }
  }

  inline void InterestPointDescriptorSurf::genSubSummedImagesUpright4x4(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col)
  {
    int x, y, ymrad, xmrad;
    float gx = 0.f, gy = 0.f, gx24 = 0.f, gy24 = 0.f, gx24abs = 0.f, gy24abs = 0.f;
    //1: (x, y)
    x = begin_x;
    y = begin_y;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //2: (x+1, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //3: (x+2, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //4: (x+3, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //5: (x, y+1)
    x = begin_x;
    y = begin_y + 1;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //6: (x+1, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //7: (x+2, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //8: (x+3, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);


    //9: (x, y+2)
    x = begin_x;
    y = begin_y + 2;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //10: (x+1, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //11: (x+2, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //12: (x+3, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //13: (x, y+3)
    x = begin_x;
    y = begin_y + 3;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //14: (x+1, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //15: (x+2, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //16: (x+3, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    _dx24[row][col] = gx24 * 0.0625f;
    _dy24[row][col] = gy24 * 0.0625f;
    _dx24abs[row][col] = gx24abs * 0.0625f;
    _dy24abs[row][col] = gy24abs * 0.0625f;
  }

  inline void InterestPointDescriptorSurf::genSummedImagesUpright4x4(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius)
  {
    int r, c, i = 0, j;
    const int haarwidth = (haarradius << 1);
    for (r = begin_y; r < end_y; r += 4, i++)
    {
      for (c = begin_x, j = 0; c < end_x; c += 4, j++)
      {
        genSubSummedImagesUpright4x4(integral, c, r, haarradius, haarwidth, i, j);
      }
    }
  }

  inline void InterestPointDescriptorSurf::genSubSummedImagesUpright5x5(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col)
  {
    int x, y, ymrad, xmrad;
    float gx = 0.f, gy = 0.f, gx24 = 0.f, gy24 = 0.f, gx24abs = 0.f, gy24abs = 0.f;
    //1: (x, y)
    x = begin_x;
    y = begin_y;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //2: (x+1, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //3: (x+2, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //4: (x+3, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //5: (x+4, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //6: (x, y+1)
    x = begin_x;
    y = begin_y + 1;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //7: (x+1, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //8: (x+2, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //9: (x+3, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //10: (x+4, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //11: (x, y+2)
    x = begin_x;
    y = begin_y + 2;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //12: (x+1, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //13: (x+2, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //14: (x+3, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //15: (x+4, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //16: (x, y+3)
    x = begin_x;
    y = begin_y + 3;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //17: (x+1, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //18: (x+2, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //19: (x+3, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //20: (x+4, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //21: (x, y+4)
    x = begin_x;
    y = begin_y + 4;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //22: (x+1, y+4)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //23: (x+2, y+4)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //24: (x+3, y+4)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //25: (x+4, y+4)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    _dx24[row][col] = gx24 * 0.04f;
    _dy24[row][col] = gy24 * 0.04f;
    _dx24abs[row][col] = gx24abs * 0.04f;
    _dy24abs[row][col] = gy24abs * 0.04f;
  }

  inline void InterestPointDescriptorSurf::genSummedImagesUpright5x5(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius)
  {
    int r, c, i = 0, j;
    const int haarwidth = (haarradius << 1);
    for (r = begin_y; r < end_y; r += 5, i++)
    {
      for (c = begin_x, j = 0; c < end_x; c += 5, j++)
      {
        genSubSummedImagesUpright5x5(integral, c, r, haarradius, haarwidth, i, j);
      }
    }
  }

  inline void InterestPointDescriptorSurf::genSubSummedImagesUpright6x6(const float *const *integral, int begin_x, int begin_y, int haarradius, int haarwidth, int row, int col)
  {
    int x, y, ymrad, xmrad;
    float gx = 0.f, gy = 0.f, gx24 = 0.f, gy24 = 0.f, gx24abs = 0.f, gy24abs = 0.f;
    //1: (x, y)
    x = begin_x;
    y = begin_y;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //2: (x+1, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //3: (x+2, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //4: (x+3, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //5: (x+4, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //6: (x+5, y)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //7: (x, y+1)
    x = begin_x;
    y = begin_y + 1;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //8: (x+1, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //9: (x+2, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //10: (x+3, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //11: (x+4, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //12: (x+5, y+1)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);



    //13: (x, y+2)
    x = begin_x;
    y = begin_y + 2;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //14: (x+1, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //15: (x+2, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //16: (x+3, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //17: (x+4, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //18: (x+5, y+2)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //19: (x, y+3)
    x = begin_x;
    y = begin_y + 3;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //20: (x+1, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //21: (x+2, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //22: (x+3, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //23: (x+4, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //24: (x+5, y+3)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    //25: (x, y+4)
    x = begin_x;
    y = begin_y + 4;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //26: (x+1, y+4)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //27: (x+2, y+4)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //28: (x+3, y+4)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //29: (x+4, y+4)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //30: (x+5, y+4)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);


    //31: (x, y+5)
    x = begin_x;
    y = begin_y + 5;
    xmrad = x - haarradius;
    ymrad = y - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //32: (x+1, y+5)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //33: (x+2, y+5)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //34: (x+3, y+5)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //35: (x+4, y+5)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);
    //36: (x+5, y+5)
    x = x + 1;
    xmrad = x - haarradius;
    gx = i_integral_rect(integral, x, ymrad, haarradius, haarwidth, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarradius, haarwidth, _width, _height);
    gy = i_integral_rect(integral, xmrad, y, haarwidth, haarradius, _width, _height) - i_integral_rect(integral, xmrad, ymrad, haarwidth, haarradius, _width, _height);
    gx24 += gx; gy24 += gy; gx24abs += i_abs(gx); gy24abs += i_abs(gy);

    static const float sf = 1.f / 36;
    _dx24[row][col] = gx24 * sf;
    _dy24[row][col] = gy24 * sf;
    _dx24abs[row][col] = gx24abs * sf;
    _dy24abs[row][col] = gy24abs * sf;
  }

  inline void InterestPointDescriptorSurf::genSummedImagesUpright6x6(const float *const *integral, int begin_x, int begin_y, int end_x, int end_y, int haarradius)
  {
    int r, c, i = 0, j;
    const int haarwidth = (haarradius << 1);
    for (r = begin_y; r < end_y; r += 6, i++)
    {
      for (c = begin_x, j = 0; c < end_x; c += 6, j++)
      {
        genSubSummedImagesUpright6x6(integral, c, r, haarradius, haarwidth, i, j);
      }
    }
  }

  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
  inline void InterestPointDescriptorSurf::encodeSubDescriptor00(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[0];
    
    dx += i_dot9(_gaussian1,      _dx24[0]);
    dx += i_dot9(_gaussian1 + 9,  _dx24[1]);
    dx += i_dot9(_gaussian1 + 18, _dx24[2]);
    dx += i_dot9(_gaussian1 + 27, _dx24[3]);
    dx += i_dot9(_gaussian1 + 36, _dx24[4]);
    dx += i_dot9(_gaussian1 + 45, _dx24[5]);
    dx += i_dot9(_gaussian1 + 54, _dx24[6]);
    dx += i_dot9(_gaussian1 + 63, _dx24[7]);
    dx += i_dot9(_gaussian1 + 72, _dx24[8]);

    dy += i_dot9(_gaussian1,      _dy24[0]);
    dy += i_dot9(_gaussian1 + 9,  _dy24[1]);
    dy += i_dot9(_gaussian1 + 18, _dy24[2]);
    dy += i_dot9(_gaussian1 + 27, _dy24[3]);
    dy += i_dot9(_gaussian1 + 36, _dy24[4]);
    dy += i_dot9(_gaussian1 + 45, _dy24[5]);
    dy += i_dot9(_gaussian1 + 54, _dy24[6]);
    dy += i_dot9(_gaussian1 + 63, _dy24[7]);
    dy += i_dot9(_gaussian1 + 72, _dy24[8]);

    dxabs += i_dot9(_gaussian1,      _dx24abs[0]);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[1]);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[2]);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[3]);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[4]);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[5]);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[6]);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[7]);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[8]);

    dyabs += i_dot9(_gaussian1,      _dy24abs[0]);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[1]);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[2]);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[3]);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[4]);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[5]);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[6]);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[7]);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[8]);
  
    ipt.descriptor[0] = dx * weight;
    ipt.descriptor[1] = dy * weight;
    ipt.descriptor[2] = dxabs * weight;
    ipt.descriptor[3] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor01(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[1];

    dx += i_dot9(_gaussian1,      _dx24[0]+5);
    dx += i_dot9(_gaussian1 + 9,  _dx24[1]+5);
    dx += i_dot9(_gaussian1 + 18, _dx24[2]+5);
    dx += i_dot9(_gaussian1 + 27, _dx24[3]+5);
    dx += i_dot9(_gaussian1 + 36, _dx24[4]+5);
    dx += i_dot9(_gaussian1 + 45, _dx24[5]+5);
    dx += i_dot9(_gaussian1 + 54, _dx24[6]+5);
    dx += i_dot9(_gaussian1 + 63, _dx24[7]+5);
    dx += i_dot9(_gaussian1 + 72, _dx24[8]+5);

    dy += i_dot9(_gaussian1,      _dy24[0]+5);
    dy += i_dot9(_gaussian1 + 9,  _dy24[1]+5);
    dy += i_dot9(_gaussian1 + 18, _dy24[2]+5);
    dy += i_dot9(_gaussian1 + 27, _dy24[3]+5);
    dy += i_dot9(_gaussian1 + 36, _dy24[4]+5);
    dy += i_dot9(_gaussian1 + 45, _dy24[5]+5);
    dy += i_dot9(_gaussian1 + 54, _dy24[6]+5);
    dy += i_dot9(_gaussian1 + 63, _dy24[7]+5);
    dy += i_dot9(_gaussian1 + 72, _dy24[8]+5);

    dxabs += i_dot9(_gaussian1,      _dx24abs[0]+5);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[1]+5);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[2]+5);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[3]+5);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[4]+5);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[5]+5);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[6]+5);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[7]+5);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[8]+5);

    dyabs += i_dot9(_gaussian1,      _dy24abs[0]+5);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[1]+5);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[2]+5);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[3]+5);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[4]+5);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[5]+5);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[6]+5);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[7]+5);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[8]+5);

    ipt.descriptor[4] = dx * weight;
    ipt.descriptor[5] = dy * weight;
    ipt.descriptor[6] = dxabs * weight;
    ipt.descriptor[7] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor02(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[2];
    
    dx += i_dot9(_gaussian1,      _dx24[0] + 10);
    dx += i_dot9(_gaussian1 + 9,  _dx24[1] + 10);
    dx += i_dot9(_gaussian1 + 18, _dx24[2] + 10);
    dx += i_dot9(_gaussian1 + 27, _dx24[3] + 10);
    dx += i_dot9(_gaussian1 + 36, _dx24[4] + 10);
    dx += i_dot9(_gaussian1 + 45, _dx24[5] + 10);
    dx += i_dot9(_gaussian1 + 54, _dx24[6] + 10);
    dx += i_dot9(_gaussian1 + 63, _dx24[7] + 10);
    dx += i_dot9(_gaussian1 + 72, _dx24[8] + 10);

    dy += i_dot9(_gaussian1,      _dy24[0] + 10);
    dy += i_dot9(_gaussian1 + 9,  _dy24[1] + 10);
    dy += i_dot9(_gaussian1 + 18, _dy24[2] + 10);
    dy += i_dot9(_gaussian1 + 27, _dy24[3] + 10);
    dy += i_dot9(_gaussian1 + 36, _dy24[4] + 10);
    dy += i_dot9(_gaussian1 + 45, _dy24[5] + 10);
    dy += i_dot9(_gaussian1 + 54, _dy24[6] + 10);
    dy += i_dot9(_gaussian1 + 63, _dy24[7] + 10);
    dy += i_dot9(_gaussian1 + 72, _dy24[8] + 10);

    dxabs += i_dot9(_gaussian1,      _dx24abs[0] + 10);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[1] + 10);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[2] + 10);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[3] + 10);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[4] + 10);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[5] + 10);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[6] + 10);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[7] + 10);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[8] + 10);

    dyabs += i_dot9(_gaussian1,      _dy24abs[0] + 10);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[1] + 10);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[2] + 10);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[3] + 10);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[4] + 10);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[5] + 10);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[6] + 10);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[7] + 10);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[8] + 10);

    ipt.descriptor[8] = dx * weight;
    ipt.descriptor[9] = dy * weight;
    ipt.descriptor[10] = dxabs * weight;
    ipt.descriptor[11] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor03(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[3];

    dx += i_dot9(_gaussian1,      _dx24[0] + 15);
    dx += i_dot9(_gaussian1 + 9,  _dx24[1] + 15);
    dx += i_dot9(_gaussian1 + 18, _dx24[2] + 15);
    dx += i_dot9(_gaussian1 + 27, _dx24[3] + 15);
    dx += i_dot9(_gaussian1 + 36, _dx24[4] + 15);
    dx += i_dot9(_gaussian1 + 45, _dx24[5] + 15);
    dx += i_dot9(_gaussian1 + 54, _dx24[6] + 15);
    dx += i_dot9(_gaussian1 + 63, _dx24[7] + 15);
    dx += i_dot9(_gaussian1 + 72, _dx24[8] + 15);

    dy += i_dot9(_gaussian1,      _dy24[0] + 15);
    dy += i_dot9(_gaussian1 + 9,  _dy24[1] + 15);
    dy += i_dot9(_gaussian1 + 18, _dy24[2] + 15);
    dy += i_dot9(_gaussian1 + 27, _dy24[3] + 15);
    dy += i_dot9(_gaussian1 + 36, _dy24[4] + 15);
    dy += i_dot9(_gaussian1 + 45, _dy24[5] + 15);
    dy += i_dot9(_gaussian1 + 54, _dy24[6] + 15);
    dy += i_dot9(_gaussian1 + 63, _dy24[7] + 15);
    dy += i_dot9(_gaussian1 + 72, _dy24[8] + 15);

    dxabs += i_dot9(_gaussian1,      _dx24abs[0] + 15);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[1] + 15);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[2] + 15);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[3] + 15);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[4] + 15);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[5] + 15);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[6] + 15);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[7] + 15);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[8] + 15);

    dyabs += i_dot9(_gaussian1,      _dy24abs[0] + 15);   
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[1] + 15);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[2] + 15);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[3] + 15);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[4] + 15);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[5] + 15);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[6] + 15);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[7] + 15);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[8] + 15);
    ipt.descriptor[12] = dx * weight;
    ipt.descriptor[13] = dy * weight;
    ipt.descriptor[14] = dxabs * weight;
    ipt.descriptor[15] = dyabs * weight;
  }

  inline void InterestPointDescriptorSurf::encodeSubDescriptor10(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[4];
    dx += i_dot9(_gaussian1,      _dx24[5]);
    dx += i_dot9(_gaussian1 + 9,  _dx24[6]);
    dx += i_dot9(_gaussian1 + 18, _dx24[7]);
    dx += i_dot9(_gaussian1 + 27, _dx24[8]);
    dx += i_dot9(_gaussian1 + 36, _dx24[9]);
    dx += i_dot9(_gaussian1 + 45, _dx24[10]);
    dx += i_dot9(_gaussian1 + 54, _dx24[11]);
    dx += i_dot9(_gaussian1 + 63, _dx24[12]);
    dx += i_dot9(_gaussian1 + 72, _dx24[13]);

    dy += i_dot9(_gaussian1,      _dy24[5]);
    dy += i_dot9(_gaussian1 + 9,  _dy24[6]);
    dy += i_dot9(_gaussian1 + 18, _dy24[7]);
    dy += i_dot9(_gaussian1 + 27, _dy24[8]);
    dy += i_dot9(_gaussian1 + 36, _dy24[9]);
    dy += i_dot9(_gaussian1 + 45, _dy24[10]);
    dy += i_dot9(_gaussian1 + 54, _dy24[11]);
    dy += i_dot9(_gaussian1 + 63, _dy24[12]);
    dy += i_dot9(_gaussian1 + 72, _dy24[13]);

    dxabs += i_dot9(_gaussian1,      _dx24abs[5]);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[6]);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[7]);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[8]);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[9]);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[10]);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[11]);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[12]);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[13]);

    dyabs += i_dot9(_gaussian1,      _dy24abs[5]);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[6]);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[7]);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[8]);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[9]);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[10]);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[11]);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[12]);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[13]);
    ipt.descriptor[16] = dx * weight;
    ipt.descriptor[17] = dy * weight;
    ipt.descriptor[18] = dxabs * weight;
    ipt.descriptor[19] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor11(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[5];

    dx += i_dot9(_gaussian1,      _dx24[5]+5);
    dx += i_dot9(_gaussian1 + 9,  _dx24[6]+5);
    dx += i_dot9(_gaussian1 + 18, _dx24[7]+5);
    dx += i_dot9(_gaussian1 + 27, _dx24[8]+5);
    dx += i_dot9(_gaussian1 + 36, _dx24[9]+5);
    dx += i_dot9(_gaussian1 + 45, _dx24[10]+5);
    dx += i_dot9(_gaussian1 + 54, _dx24[11]+5);
    dx += i_dot9(_gaussian1 + 63, _dx24[12]+5);
    dx += i_dot9(_gaussian1 + 72, _dx24[13]+5);

    dy += i_dot9(_gaussian1,      _dy24[5]+5);
    dy += i_dot9(_gaussian1 + 9,  _dy24[6]+5);
    dy += i_dot9(_gaussian1 + 18, _dy24[7]+5);
    dy += i_dot9(_gaussian1 + 27, _dy24[8]+5);
    dy += i_dot9(_gaussian1 + 36, _dy24[9]+5);
    dy += i_dot9(_gaussian1 + 45, _dy24[10]+5);
    dy += i_dot9(_gaussian1 + 54, _dy24[11]+5);
    dy += i_dot9(_gaussian1 + 63, _dy24[12]+5);
    dy += i_dot9(_gaussian1 + 72, _dy24[13]+5);

    dxabs += i_dot9(_gaussian1, _dx24abs[5] + 5);
    dxabs += i_dot9(_gaussian1 + 9, _dx24abs[6] + 5);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[7] + 5);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[8] + 5);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[9]+5);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[10]+5);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[11]+5);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[12]+5);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[13]+5);

    dyabs += i_dot9(_gaussian1,      _dy24abs[5]+5);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[6]+5);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[7]+5);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[8]+5);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[9]+5);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[10]+5);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[11]+5);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[12]+5);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[13]+5);

    ipt.descriptor[20] = dx * weight;
    ipt.descriptor[21] = dy * weight;
    ipt.descriptor[22] = dxabs * weight;
    ipt.descriptor[23] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor12(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[6];

    dx += i_dot9(_gaussian1,      _dx24[5] + 10);
    dx += i_dot9(_gaussian1 + 9,  _dx24[6] + 10);
    dx += i_dot9(_gaussian1 + 18, _dx24[7] + 10);
    dx += i_dot9(_gaussian1 + 27, _dx24[8] + 10);
    dx += i_dot9(_gaussian1 + 36, _dx24[9] + 10);
    dx += i_dot9(_gaussian1 + 45, _dx24[10] + 10);
    dx += i_dot9(_gaussian1 + 54, _dx24[11] + 10);
    dx += i_dot9(_gaussian1 + 63, _dx24[12] + 10);
    dx += i_dot9(_gaussian1 + 72, _dx24[13] + 10);

    dy += i_dot9(_gaussian1,      _dy24[5] + 10);
    dy += i_dot9(_gaussian1 + 9,  _dy24[6] + 10);
    dy += i_dot9(_gaussian1 + 18, _dy24[7] + 10);
    dy += i_dot9(_gaussian1 + 27, _dy24[8] + 10);
    dy += i_dot9(_gaussian1 + 36, _dy24[9] + 10);
    dy += i_dot9(_gaussian1 + 45, _dy24[10] + 10);
    dy += i_dot9(_gaussian1 + 54, _dy24[11] + 10);
    dy += i_dot9(_gaussian1 + 63, _dy24[12] + 10);
    dy += i_dot9(_gaussian1 + 72, _dy24[13] + 10);

    dxabs += i_dot9(_gaussian1,      _dx24abs[5] + 10);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[6] + 10);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[7] + 10);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[8] + 10);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[9] + 10);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[10] + 10);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[11] + 10);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[12] + 10);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[13] + 10);

    dyabs += i_dot9(_gaussian1,      _dy24abs[5] + 10);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[6] + 10);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[7] + 10);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[8] + 10);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[9] + 10);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[10] + 10);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[11] + 10);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[12] + 10);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[13] + 10);

    ipt.descriptor[24] = dx * weight;
    ipt.descriptor[25] = dy * weight;
    ipt.descriptor[26] = dxabs * weight;
    ipt.descriptor[27] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor13(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[7];
    
    dx += i_dot9(_gaussian1,      _dx24[5] + 15);
    dx += i_dot9(_gaussian1 + 9,  _dx24[6] + 15);
    dx += i_dot9(_gaussian1 + 18, _dx24[7] + 15);
    dx += i_dot9(_gaussian1 + 27, _dx24[8] + 15);
    dx += i_dot9(_gaussian1 + 36, _dx24[9] + 15);
    dx += i_dot9(_gaussian1 + 45, _dx24[10] + 15);
    dx += i_dot9(_gaussian1 + 54, _dx24[11] + 15);
    dx += i_dot9(_gaussian1 + 63, _dx24[12] + 15);
    dx += i_dot9(_gaussian1 + 72, _dx24[13] + 15);

    dy += i_dot9(_gaussian1,      _dy24[5] + 15);
    dy += i_dot9(_gaussian1 + 9,  _dy24[6] + 15);
    dy += i_dot9(_gaussian1 + 18, _dy24[7] + 15);
    dy += i_dot9(_gaussian1 + 27, _dy24[8] + 15);
    dy += i_dot9(_gaussian1 + 36, _dy24[9] + 15);
    dy += i_dot9(_gaussian1 + 45, _dy24[10] + 15);
    dy += i_dot9(_gaussian1 + 54, _dy24[11] + 15);
    dy += i_dot9(_gaussian1 + 63, _dy24[12] + 15);
    dy += i_dot9(_gaussian1 + 72, _dy24[13] + 15);

    dxabs += i_dot9(_gaussian1,      _dx24abs[5] + 15);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[6] + 15);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[7] + 15);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[8] + 15);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[9] + 15);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[10] + 15);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[11] + 15);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[12] + 15);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[13] + 15);

    dyabs += i_dot9(_gaussian1,      _dy24abs[5] + 15);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[6] + 15);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[7] + 15);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[8] + 15);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[9] + 15);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[10] + 15);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[11] + 15);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[12] + 15);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[13] + 15);

    ipt.descriptor[28] = dx * weight;
    ipt.descriptor[29] = dy * weight;
    ipt.descriptor[30] = dxabs * weight;
    ipt.descriptor[31] = dyabs * weight;
  }

  inline void InterestPointDescriptorSurf::encodeSubDescriptor20(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[8];

    dx += i_dot9(_gaussian1,      _dx24[10]);
    dx += i_dot9(_gaussian1 + 9,  _dx24[11]);
    dx += i_dot9(_gaussian1 + 18, _dx24[12]);
    dx += i_dot9(_gaussian1 + 27, _dx24[13]);
    dx += i_dot9(_gaussian1 + 36, _dx24[14]);
    dx += i_dot9(_gaussian1 + 45, _dx24[15]);
    dx += i_dot9(_gaussian1 + 54, _dx24[16]);
    dx += i_dot9(_gaussian1 + 63, _dx24[17]);
    dx += i_dot9(_gaussian1 + 72, _dx24[18]);

    dy += i_dot9(_gaussian1,      _dy24[10]);
    dy += i_dot9(_gaussian1 + 9,  _dy24[11]);
    dy += i_dot9(_gaussian1 + 18, _dy24[12]);
    dy += i_dot9(_gaussian1 + 27, _dy24[13]);
    dy += i_dot9(_gaussian1 + 36, _dy24[14]);
    dy += i_dot9(_gaussian1 + 45, _dy24[15]);
    dy += i_dot9(_gaussian1 + 54, _dy24[16]);
    dy += i_dot9(_gaussian1 + 63, _dy24[17]);
    dy += i_dot9(_gaussian1 + 72, _dy24[18]);

    dxabs += i_dot9(_gaussian1,      _dx24abs[10]);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[11]);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[12]);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[13]);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[14]);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[15]);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[16]);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[17]);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[18]);

    dyabs += i_dot9(_gaussian1,      _dy24abs[10]);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[11]);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[12]);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[13]);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[14]);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[15]);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[16]);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[17]);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[18]);
    
    ipt.descriptor[32] = dx * weight;
    ipt.descriptor[33] = dy * weight;
    ipt.descriptor[34] = dxabs * weight;
    ipt.descriptor[35] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor21(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[9];

    dx += i_dot9(_gaussian1,      _dx24[10]+5);
    dx += i_dot9(_gaussian1 + 9,  _dx24[11]+5);
    dx += i_dot9(_gaussian1 + 18, _dx24[12]+5);
    dx += i_dot9(_gaussian1 + 27, _dx24[13]+5);
    dx += i_dot9(_gaussian1 + 36, _dx24[14]+5);
    dx += i_dot9(_gaussian1 + 45, _dx24[15]+5);
    dx += i_dot9(_gaussian1 + 54, _dx24[16]+5);
    dx += i_dot9(_gaussian1 + 63, _dx24[17]+5);
    dx += i_dot9(_gaussian1 + 72, _dx24[18]+5);

    dy += i_dot9(_gaussian1,      _dy24[10]+5);
    dy += i_dot9(_gaussian1 + 9,  _dy24[11]+5);
    dy += i_dot9(_gaussian1 + 18, _dy24[12]+5);
    dy += i_dot9(_gaussian1 + 27, _dy24[13]+5);
    dy += i_dot9(_gaussian1 + 36, _dy24[14]+5);
    dy += i_dot9(_gaussian1 + 45, _dy24[15]+5);
    dy += i_dot9(_gaussian1 + 54, _dy24[16]+5);
    dy += i_dot9(_gaussian1 + 63, _dy24[17]+5);
    dy += i_dot9(_gaussian1 + 72, _dy24[18]+5);

    dxabs += i_dot9(_gaussian1,      _dx24abs[10]+5);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[11]+5);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[12]+5);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[13]+5);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[14]+5);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[15]+5);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[16]+5);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[17]+5);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[18]+5);

    dyabs += i_dot9(_gaussian1,      _dy24abs[10]+5);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[11]+5);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[12]+5);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[13]+5);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[14]+5);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[15]+5);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[16]+5);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[17]+5);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[18]+5);

    ipt.descriptor[36] = dx * weight;
    ipt.descriptor[37] = dy * weight;
    ipt.descriptor[38] = dxabs * weight;
    ipt.descriptor[39] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor22(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[10];

    dx += i_dot9(_gaussian1,      _dx24[10] + 10);
    dx += i_dot9(_gaussian1 + 9,  _dx24[11] + 10);
    dx += i_dot9(_gaussian1 + 18, _dx24[12] + 10);
    dx += i_dot9(_gaussian1 + 27, _dx24[13] + 10);
    dx += i_dot9(_gaussian1 + 36, _dx24[14] + 10);
    dx += i_dot9(_gaussian1 + 45, _dx24[15] + 10);
    dx += i_dot9(_gaussian1 + 54, _dx24[16] + 10);
    dx += i_dot9(_gaussian1 + 63, _dx24[17] + 10);
    dx += i_dot9(_gaussian1 + 72, _dx24[18] + 10);

    dy += i_dot9(_gaussian1,      _dy24[10] + 10);
    dy += i_dot9(_gaussian1 + 9,  _dy24[11] + 10);
    dy += i_dot9(_gaussian1 + 18, _dy24[12] + 10);
    dy += i_dot9(_gaussian1 + 27, _dy24[13] + 10);
    dy += i_dot9(_gaussian1 + 36, _dy24[14] + 10);
    dy += i_dot9(_gaussian1 + 45, _dy24[15] + 10);
    dy += i_dot9(_gaussian1 + 54, _dy24[16] + 10);
    dy += i_dot9(_gaussian1 + 63, _dy24[17] + 10);
    dy += i_dot9(_gaussian1 + 72, _dy24[18] + 10);

    dxabs += i_dot9(_gaussian1,      _dx24abs[10] + 10);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[11] + 10);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[12] + 10);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[13] + 10);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[14] + 10);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[15] + 10);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[16] + 10);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[17] + 10);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[18] + 10);

    dyabs += i_dot9(_gaussian1,      _dy24abs[10] + 10);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[11] + 10);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[12] + 10);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[13] + 10);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[14] + 10);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[15] + 10);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[16] + 10);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[17] + 10);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[18] + 10);

    ipt.descriptor[40] = dx * weight;
    ipt.descriptor[41] = dy * weight;
    ipt.descriptor[42] = dxabs * weight;
    ipt.descriptor[43] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor23(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[11];

    dx += i_dot9(_gaussian1,      _dx24[10] + 15);
    dx += i_dot9(_gaussian1 + 9,  _dx24[11] + 15);
    dx += i_dot9(_gaussian1 + 18, _dx24[12] + 15);
    dx += i_dot9(_gaussian1 + 27, _dx24[13] + 15);
    dx += i_dot9(_gaussian1 + 36, _dx24[14] + 15);
    dx += i_dot9(_gaussian1 + 45, _dx24[15] + 15);
    dx += i_dot9(_gaussian1 + 54, _dx24[16] + 15);
    dx += i_dot9(_gaussian1 + 63, _dx24[17] + 15);
    dx += i_dot9(_gaussian1 + 72, _dx24[18] + 15);

    dy += i_dot9(_gaussian1,      _dy24[10] + 15);
    dy += i_dot9(_gaussian1 + 9,  _dy24[11] + 15);
    dy += i_dot9(_gaussian1 + 18, _dy24[12] + 15);
    dy += i_dot9(_gaussian1 + 27, _dy24[13] + 15);
    dy += i_dot9(_gaussian1 + 36, _dy24[14] + 15);
    dy += i_dot9(_gaussian1 + 45, _dy24[15] + 15);
    dy += i_dot9(_gaussian1 + 54, _dy24[16] + 15);
    dy += i_dot9(_gaussian1 + 63, _dy24[17] + 15);
    dy += i_dot9(_gaussian1 + 72, _dy24[18] + 15);

    dxabs += i_dot9(_gaussian1,      _dx24abs[10] + 15);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[11] + 15);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[12] + 15);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[13] + 15);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[14] + 15);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[15] + 15);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[16] + 15);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[17] + 15);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[18] + 15);

    dyabs += i_dot9(_gaussian1,      _dy24abs[10] + 15);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[11] + 15);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[12] + 15);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[13] + 15);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[14] + 15);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[15] + 15);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[16] + 15);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[17] + 15);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[18] + 15);

    ipt.descriptor[44] = dx * weight;
    ipt.descriptor[45] = dy * weight;
    ipt.descriptor[46] = dxabs * weight;
    ipt.descriptor[47] = dyabs * weight;
  }

  inline void InterestPointDescriptorSurf::encodeSubDescriptor30(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[12];

    dx += i_dot9(_gaussian1,      _dx24[15]);
    dx += i_dot9(_gaussian1 + 9,  _dx24[16]);
    dx += i_dot9(_gaussian1 + 18, _dx24[17]);
    dx += i_dot9(_gaussian1 + 27, _dx24[18]);
    dx += i_dot9(_gaussian1 + 36, _dx24[19]);
    dx += i_dot9(_gaussian1 + 45, _dx24[20]);
    dx += i_dot9(_gaussian1 + 54, _dx24[21]);
    dx += i_dot9(_gaussian1 + 63, _dx24[22]);
    dx += i_dot9(_gaussian1 + 72, _dx24[23]);

    dy += i_dot9(_gaussian1,      _dy24[15]);
    dy += i_dot9(_gaussian1 + 9,  _dy24[16]);
    dy += i_dot9(_gaussian1 + 18, _dy24[17]);
    dy += i_dot9(_gaussian1 + 27, _dy24[18]);
    dy += i_dot9(_gaussian1 + 36, _dy24[19]);
    dy += i_dot9(_gaussian1 + 45, _dy24[20]);
    dy += i_dot9(_gaussian1 + 54, _dy24[21]);
    dy += i_dot9(_gaussian1 + 63, _dy24[22]);
    dy += i_dot9(_gaussian1 + 72, _dy24[23]);

    dxabs += i_dot9(_gaussian1,      _dx24abs[15]);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[16]);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[17]);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[18]);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[19]);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[20]);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[21]);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[22]);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[23]);

    dyabs += i_dot9(_gaussian1,      _dy24abs[15]);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[16]);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[17]);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[18]);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[19]);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[20]);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[21]);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[22]);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[23]);

    ipt.descriptor[48] = dx * weight;
    ipt.descriptor[49] = dy * weight;
    ipt.descriptor[50] = dxabs * weight;
    ipt.descriptor[51] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor31(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[13];

    dx += i_dot9(_gaussian1,      _dx24[15]+5);
    dx += i_dot9(_gaussian1 + 9,  _dx24[16]+5);
    dx += i_dot9(_gaussian1 + 18, _dx24[17]+5);
    dx += i_dot9(_gaussian1 + 27, _dx24[18]+5);
    dx += i_dot9(_gaussian1 + 36, _dx24[19]+5);
    dx += i_dot9(_gaussian1 + 45, _dx24[20]+5);
    dx += i_dot9(_gaussian1 + 54, _dx24[21]+5);
    dx += i_dot9(_gaussian1 + 63, _dx24[22]+5);
    dx += i_dot9(_gaussian1 + 72, _dx24[23]+5);

    dy += i_dot9(_gaussian1,      _dy24[15]+5);
    dy += i_dot9(_gaussian1 + 9,  _dy24[16]+5);
    dy += i_dot9(_gaussian1 + 18, _dy24[17]+5);
    dy += i_dot9(_gaussian1 + 27, _dy24[18]+5);
    dy += i_dot9(_gaussian1 + 36, _dy24[19]+5);
    dy += i_dot9(_gaussian1 + 45, _dy24[20]+5);
    dy += i_dot9(_gaussian1 + 54, _dy24[21]+5);
    dy += i_dot9(_gaussian1 + 63, _dy24[22]+5);
    dy += i_dot9(_gaussian1 + 72, _dy24[23]+5);

    dxabs += i_dot9(_gaussian1,      _dx24abs[15]+5);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[16]+5);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[17]+5);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[18]+5);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[19]+5);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[20]+5);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[21]+5);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[22]+5);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[23]+5);

    dyabs += i_dot9(_gaussian1,      _dy24abs[15]+5);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[16]+5);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[17]+5);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[18]+5);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[19]+5);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[20]+5);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[21]+5);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[22]+5);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[23]+5);

    ipt.descriptor[52] = dx * weight;
    ipt.descriptor[53] = dy * weight;
    ipt.descriptor[54] = dxabs * weight;
    ipt.descriptor[55] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor32(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[14];
    dx += i_dot9(_gaussian1,      _dx24[15] + 10);
    dx += i_dot9(_gaussian1 + 9,  _dx24[16] + 10);
    dx += i_dot9(_gaussian1 + 18, _dx24[17] + 10);
    dx += i_dot9(_gaussian1 + 27, _dx24[18] + 10);
    dx += i_dot9(_gaussian1 + 36, _dx24[19] + 10);
    dx += i_dot9(_gaussian1 + 45, _dx24[20] + 10);
    dx += i_dot9(_gaussian1 + 54, _dx24[21] + 10);
    dx += i_dot9(_gaussian1 + 63, _dx24[22] + 10);
    dx += i_dot9(_gaussian1 + 72, _dx24[23] + 10);

    dy += i_dot9(_gaussian1,      _dy24[15] + 10);
    dy += i_dot9(_gaussian1 + 9,  _dy24[16] + 10);
    dy += i_dot9(_gaussian1 + 18, _dy24[17] + 10);
    dy += i_dot9(_gaussian1 + 27, _dy24[18] + 10);
    dy += i_dot9(_gaussian1 + 36, _dy24[19] + 10);
    dy += i_dot9(_gaussian1 + 45, _dy24[20] + 10);
    dy += i_dot9(_gaussian1 + 54, _dy24[21] + 10);
    dy += i_dot9(_gaussian1 + 63, _dy24[22] + 10);
    dy += i_dot9(_gaussian1 + 72, _dy24[23] + 10);

    dxabs += i_dot9(_gaussian1,      _dx24abs[15] + 10);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[16] + 10);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[17] + 10);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[18] + 10);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[19] + 10);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[20] + 10);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[21] + 10);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[22] + 10);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[23] + 10);

    dyabs += i_dot9(_gaussian1,      _dy24abs[15] + 10);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[16] + 10);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[17] + 10);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[18] + 10);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[19] + 10);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[20] + 10);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[21] + 10);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[22] + 10);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[23] + 10);

    ipt.descriptor[56] = dx * weight;
    ipt.descriptor[57] = dy * weight;
    ipt.descriptor[58] = dxabs * weight;
    ipt.descriptor[59] = dyabs * weight;
  }
  inline void InterestPointDescriptorSurf::encodeSubDescriptor33(InterestPointFastHessian &ipt)
  {
    float dx = 0.f, dy = 0.f, dxabs = 0.f, dyabs = 0.f, weight = _gaussian2[15];

    dx += i_dot9(_gaussian1,      _dx24[15] + 15);
    dx += i_dot9(_gaussian1 + 9,  _dx24[16] + 15);
    dx += i_dot9(_gaussian1 + 18, _dx24[17] + 15);
    dx += i_dot9(_gaussian1 + 27, _dx24[18] + 15);
    dx += i_dot9(_gaussian1 + 36, _dx24[19] + 15);
    dx += i_dot9(_gaussian1 + 45, _dx24[20] + 15);
    dx += i_dot9(_gaussian1 + 54, _dx24[21] + 15);
    dx += i_dot9(_gaussian1 + 63, _dx24[22] + 15);
    dx += i_dot9(_gaussian1 + 72, _dx24[23] + 15);

    dy += i_dot9(_gaussian1,      _dy24[15] + 15);
    dy += i_dot9(_gaussian1 + 9,  _dy24[16] + 15);
    dy += i_dot9(_gaussian1 + 18, _dy24[17] + 15);
    dy += i_dot9(_gaussian1 + 27, _dy24[18] + 15);
    dy += i_dot9(_gaussian1 + 36, _dy24[19] + 15);
    dy += i_dot9(_gaussian1 + 45, _dy24[20] + 15);
    dy += i_dot9(_gaussian1 + 54, _dy24[21] + 15);
    dy += i_dot9(_gaussian1 + 63, _dy24[22] + 15);
    dy += i_dot9(_gaussian1 + 72, _dy24[23] + 15);

    dxabs += i_dot9(_gaussian1,      _dx24abs[15] + 15);
    dxabs += i_dot9(_gaussian1 + 9,  _dx24abs[16] + 15);
    dxabs += i_dot9(_gaussian1 + 18, _dx24abs[17] + 15);
    dxabs += i_dot9(_gaussian1 + 27, _dx24abs[18] + 15);
    dxabs += i_dot9(_gaussian1 + 36, _dx24abs[19] + 15);
    dxabs += i_dot9(_gaussian1 + 45, _dx24abs[20] + 15);
    dxabs += i_dot9(_gaussian1 + 54, _dx24abs[21] + 15);
    dxabs += i_dot9(_gaussian1 + 63, _dx24abs[22] + 15);
    dxabs += i_dot9(_gaussian1 + 72, _dx24abs[23] + 15);

    dyabs += i_dot9(_gaussian1,      _dy24abs[15] + 15);
    dyabs += i_dot9(_gaussian1 + 9,  _dy24abs[16] + 15);
    dyabs += i_dot9(_gaussian1 + 18, _dy24abs[17] + 15);
    dyabs += i_dot9(_gaussian1 + 27, _dy24abs[18] + 15);
    dyabs += i_dot9(_gaussian1 + 36, _dy24abs[19] + 15);
    dyabs += i_dot9(_gaussian1 + 45, _dy24abs[20] + 15);
    dyabs += i_dot9(_gaussian1 + 54, _dy24abs[21] + 15);
    dyabs += i_dot9(_gaussian1 + 63, _dy24abs[22] + 15);
    dyabs += i_dot9(_gaussian1 + 72, _dy24abs[23] + 15);
    ipt.descriptor[60] = dx * weight;
    ipt.descriptor[61] = dy * weight;
    ipt.descriptor[62] = dxabs * weight;
    ipt.descriptor[63] = dyabs * weight;
  }

#ifndef I_PLATFORM_NO_SSE_SUPPORT
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /*SURF Feature descriptor matching functions*/
  int i_match_interestpoints_fasthessian_sse(const std::vector<InterestPointFastHessian> &ipts_1, const std::vector<InterestPointFastHessian> &ipts_2, std::vector<Pair<InterestPointFastHessian, InterestPointFastHessian> >& correspondences, double ratio_thre)
  {
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }
    int i, j, k, bestpos, type;
    float best, secondbest, dist;
    __m128 v_1, v_2, v_d, vd_2;

    const int n = ipts_1[0].dim();
    assert(n == ipts_2[0].dim());

    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<float>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        vd_2 = _mm_set1_ps(0.0f);
        for (k = 0; k < 64; k += 4)
        {
          v_1 = _mm_loadu_ps(&ipts_1[i].descriptor[k]);
          v_2 = _mm_loadu_ps(&ipts_2[j].descriptor[k]);
          v_d = _mm_sub_ps(v_1, v_2);  /*difference*/
          v_d = _mm_mul_ps(v_d, v_d);
          vd_2 = _mm_add_ps(vd_2, v_d);
        }
        // horizontal sum of 4 partial dot products
        vd_2 = _mm_hadd_ps(vd_2, vd_2);
        vd_2 = _mm_hadd_ps(vd_2, vd_2);
        _mm_store_ss(&dist, vd_2);
        dist = i_sqrt(dist);

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }
      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(ipts_1[i], ipts_2[bestpos]));
      }
    }
    return (int)correspondences.size();
  }

  int i_match_interestpoints_fasthessian_sse(const std::vector<InterestPointFastHessian> &ipts_1, const std::vector<InterestPointFastHessian> &ipts_2, std::vector<Pair<int, int> >& correspondences, double ratio_thre)
  {
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }
    int i, j, k, bestpos, type;
    float best, secondbest, dist;
    __m128 v_1, v_2, v_d, vd_2;

    const int n = ipts_1[0].dim();
    assert(n == ipts_2[0].dim());

    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<float>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }
        
        vd_2 = _mm_set1_ps(0.0f);
        for (k = 0; k < 64; k += 4)
        {
          v_1 = _mm_loadu_ps(&ipts_1[i].descriptor[k]);
          v_2 = _mm_loadu_ps(&ipts_2[j].descriptor[k]);
          v_d = _mm_sub_ps(v_1, v_2);  /*difference*/
          v_d = _mm_mul_ps(v_d, v_d);
          vd_2 = _mm_add_ps(vd_2, v_d);
        }
        // horizontal sum of 4 partial dot products
        vd_2 = _mm_hadd_ps(vd_2, vd_2);
        vd_2 = _mm_hadd_ps(vd_2, vd_2);
        _mm_store_ss(&dist, vd_2);
        dist = i_sqrt(dist);

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }
      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(i, bestpos));
      }
    }
    return (int)correspondences.size();
  }

  int i_match_interestpoints_fasthessian_sse_uniqiue(const std::vector<InterestPointFastHessian> &ipts_1,
                                                     const std::vector<InterestPointFastHessian> &ipts_2,
                                                     std::vector<Pair<InterestPointFastHessian, InterestPointFastHessian> >& correspondences,
                                                     double ratio_thre)
  {
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }

    int i, j, k, bestpos, type;
    float best, secondbest, dist;
    __m128 v_1, v_2, v_d, vd_2;

    const int n = ipts_1[0].dim();
    assert(n == ipts_2[0].dim());

    std::vector<Pair<int, float> > record(ipts_2.size());
    for (i = 0; i < (int)ipts_2.size(); i++)
    {
      record.at(i).first  = -1; /*empty - no match*/
      record.at(i).second = Constant<float>::MAX_VAL(); /*infinity*/
    }
    
    /*first pass*/
    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<float>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        /*using sse to compute the distance*/
        vd_2 = _mm_set1_ps(0.0f);
        for (k = 0; k < 64; k += 4)
        {
          v_1 = _mm_loadu_ps(&ipts_1[i].descriptor[k]);
          v_2 = _mm_loadu_ps(&ipts_2[j].descriptor[k]);
          v_d = _mm_sub_ps(v_1, v_2);  /*difference*/
          v_d = _mm_mul_ps(v_d, v_d);
          vd_2 = _mm_add_ps(vd_2, v_d);
        }
        vd_2 = _mm_hadd_ps(vd_2, vd_2); /*horizontal sum of 4 partial dot products*/
        vd_2 = _mm_hadd_ps(vd_2, vd_2); /*horizontal sum of 4 partial dot products*/
        _mm_store_ss(&dist, vd_2);
        dist = i_sqrt(dist);
        /*===================================*/

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }
      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) /*bestpos can be negative, when all types are not consistent*/
      {
        // record the match
        if (best < record.at(bestpos).second)
        {
          record.at(bestpos).first = i;
          record.at(bestpos).second = best;
        }
      }
    }

    /*second pass*/
    for (j = 0; j < (int)ipts_2.size(); j++)
    {
      i = record.at(j).first;
      if (i >= 0)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(ipts_1[i], ipts_2[j]));
      }
    }

    return (int)correspondences.size();
  }

  int i_match_interestpoints_fasthessian_sse_uniqiue(const std::vector<InterestPointFastHessian> &ipts_1,
                                                     const std::vector<InterestPointFastHessian> &ipts_2,
                                                     std::vector<Pair<int, int> >& correspondences,
                                                     double ratio_thre)
  {
    correspondences.clear();
    correspondences.reserve(ipts_1.size());
    if (ipts_1.empty() || ipts_2.empty())
    {
      return 0;
    }

    int i, j, k, bestpos, type;
    float best, secondbest, dist;
    __m128 v_1, v_2, v_d, vd_2;

    const int n = ipts_1[0].dim();
    assert(n == ipts_2[0].dim());

    std::vector<Pair<int, float> > record(ipts_2.size());
    for (i = 0; i < (int)ipts_2.size(); i++)
    {
      record.at(i).first = -1; /*empty - no match*/
      record.at(i).second = Constant<float>::MAX_VAL(); /*infinity*/
    }

    /*first pass*/
    for (i = 0; i < (int)ipts_1.size(); i++)
    {
      best = secondbest = Constant<float>::MAX_VAL();
      bestpos = -1;
      type = ipts_1[i].type;
      for (j = 0; j < (int)ipts_2.size(); j++)
      {
        if (type != ipts_2[j].type)
        {
          continue;
        }

        /*using sse to compute the distance*/
        vd_2 = _mm_set1_ps(0.0f);
        for (k = 0; k < 64; k += 4)
        {
          v_1 = _mm_loadu_ps(&ipts_1[i].descriptor[k]);
          v_2 = _mm_loadu_ps(&ipts_2[j].descriptor[k]);
          v_d = _mm_sub_ps(v_1, v_2);  /*difference*/
          v_d = _mm_mul_ps(v_d, v_d);
          vd_2 = _mm_add_ps(vd_2, v_d);
        }
        vd_2 = _mm_hadd_ps(vd_2, vd_2); /*horizontal sum of 4 partial dot products*/
        vd_2 = _mm_hadd_ps(vd_2, vd_2); /*horizontal sum of 4 partial dot products*/
        _mm_store_ss(&dist, vd_2);
        dist = i_sqrt(dist);
        /*===================================*/

        if (dist < best) // if this feature matches better than current best
        {
          bestpos = j;
          secondbest = best;
          best = dist;
        }
        else
        {
          if (dist < secondbest) // this feature matches better than second best
          {
            secondbest = dist;
          }
        }
      }
      // If match has a best:secondbest ratio < ratio_thre then consider as a match
      if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) /*bestpos can be negative, when all types are not consistent*/
      {
        // record the match
        if (best < record.at(bestpos).second)
        {
          record.at(bestpos).first = i;
          record.at(bestpos).second = best;
        }
      }
    }

    /*second pass*/
    for (j = 0; j < (int)ipts_2.size(); j++)
    {
      i = record.at(j).first;
      if (i >= 0)
      {
        // push the match to the list
        correspondences.push_back(i_make_pair(i, j));
      }
    }

    return (int)correspondences.size();
  }

  int i_match_interestpoints_fasthessian(const std::vector<InterestPointFastHessian> &ipts_1,
                                         const std::vector<InterestPointFastHessian> &ipts_2,
                                         std::vector<Pair<InterestPointFastHessian, InterestPointFastHessian> >& correspondences,
                                         double ratio_thre,
                                         int force_one_to_one,
                                         int force_geometry,
                                         double epi_dist_thre/*in pixel, suggested value is sqrt(2), can be larger for large images*/,
                                         double *fundamental)
  {
    int i, nr_correspondences = 0;
    std::vector<bool> survivors;

    if (!force_one_to_one && !force_geometry)
    {
      return i_match_interestpoints_fasthessian_sse(ipts_1, ipts_2, correspondences, ratio_thre);
    }
    else if (force_one_to_one && !force_geometry)
    {
      return i_match_interestpoints_fasthessian_sse_uniqiue(ipts_1, ipts_2, correspondences, ratio_thre);
    }
    else if (!force_one_to_one && force_geometry)
    {
      i_match_interestpoints_fasthessian_sse(ipts_1, ipts_2, correspondences, ratio_thre);
      /*geometry verification*/
      nr_correspondences = i_correspondences_epipolar_geometry_verification<InterestPointFastHessian, float>(correspondences, survivors, epi_dist_thre, fundamental);
    }
    else /*force_one_to_one && force_geometry*/
    {
      i_match_interestpoints_fasthessian_sse_uniqiue(ipts_1, ipts_2, correspondences, ratio_thre);
      /*geometry verification*/
      nr_correspondences = i_correspondences_epipolar_geometry_verification<InterestPointFastHessian, float>(correspondences, survivors, epi_dist_thre, fundamental);
    }

    std::vector<Pair<InterestPointFastHessian, InterestPointFastHessian> > correspondences_tmp;

    if (nr_correspondences < (int)correspondences.size())
    {
      for (i = 0; i < (int)correspondences.size(); ++i)
      {
        if (true == survivors.at(i))
        {
          // push the match to the list
          correspondences_tmp.push_back(correspondences.at(i));
        }
      }
      correspondences.clear();
      correspondences.assign(correspondences_tmp.begin(), correspondences_tmp.end());
    }
    return nr_correspondences;
  }

  int i_match_interestpoints_fasthessian(const std::vector<InterestPointFastHessian> &ipts_1,
                                         const std::vector<InterestPointFastHessian> &ipts_2,
                                         std::vector<Pair<int, int> >& correspondences,
                                         double ratio_thre,
                                         int force_one_to_one,
                                         int force_geometry,
                                         double epi_dist_thre/*in pixel, suggested value is sqrt(2), can be larger for large images*/,
                                         double *fundamental)
  {
    int i, nr_correspondences = 0;
    std::vector<bool> survivors;

    if (!force_one_to_one && !force_geometry)
    {
      return i_match_interestpoints_fasthessian_sse(ipts_1, ipts_2, correspondences, ratio_thre);
    }
    else if (force_one_to_one && !force_geometry)
    {
      return i_match_interestpoints_fasthessian_sse_uniqiue(ipts_1, ipts_2, correspondences, ratio_thre);
    }
    else if (!force_one_to_one && force_geometry)
    {
      i_match_interestpoints_fasthessian_sse(ipts_1, ipts_2, correspondences, ratio_thre);
      /*geometry verification*/
      nr_correspondences = i_correspondences_epipolar_geometry_verification<InterestPointFastHessian, float>(ipts_1, ipts_2, correspondences, survivors, epi_dist_thre, fundamental);
    }
    else /*force_one_to_one && force_geometry*/
    {
      i_match_interestpoints_fasthessian_sse_uniqiue(ipts_1, ipts_2, correspondences, ratio_thre);
      /*geometry verification*/
      nr_correspondences = i_correspondences_epipolar_geometry_verification<InterestPointFastHessian, float>(ipts_1, ipts_2, correspondences, survivors, epi_dist_thre, fundamental);
    }

    std::vector<Pair<int, int> > correspondences_tmp;

    if (nr_correspondences < (int)correspondences.size())
    {
      for (i = 0; i < (int)correspondences.size(); ++i)
      {
        if (true == survivors.at(i))
        {
          // push the match to the list
          correspondences_tmp.push_back(correspondences.at(i));
        }
      }
      correspondences.clear();
      correspondences.assign(correspondences_tmp.begin(), correspondences_tmp.end());
    }
    return nr_correspondences;
  }
#endif

}/*namespace idl*/
